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I.   INTRODUCTION 

A.   GENERAL 

In  recent  years,  the  focus  on  Autonomous  Underwater 
Vehicles  (AUV's)  or,  more  generally,  Unmanned  Underwater 
Vehicles  (UUV's)  has  increased.   A  variety  of  unclassified 
missions  includes  Search  and  Survey,  Decoy  and  Outboard 
sensors,  Ocean  Engineering  Work  Service,  Swimmer  Support, 
and  Test  and  Evaluations  [Ref .  1] .   As  the  cost  of  manned 
submarine  vehicles  increases,  there  are  significant 
advantages  to  the  use  of  cheaper  unmanned  vehicles.   UUV's 
can  be  either  tethered  or  untethered.   Development  in  both 
areas  is  proceeding,  but,  while  tethered  vehicles  can  use 
fiber  optic  links  to  human  operators  on  a  mother  ship,  a 
fully  autonomous  vehicle  is  required  to  have  a  high  level  of 
intelligent  processing  on  board.   Thus  the  requirements  for 
AI  and  Knowledge-Based  Controls  are  much  increased.   A 
recent  symposium  [Ref.  2]  has  presented  a  summary  of  the 
State  of  the  Art  in  Unmanned  Untethered  Submersible 
Technology. 

The  organization  of  the  intelligent  control  of  an  AUV 
can  be  expressed  as  a  cycle  of  Sensing,  Thinking,  and  Acting 
(Figure  1.1).   At  the  highest  level  of  the  control 
architecture,  the  mission  planning  and  symbolic  reasoning 
lead  to  requirements  for  path  planning  and  control.   The 
lower  level  of  Acting  involves  operation  and  control  of  all 
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Figure  1.1   Organization  of  Intelligent  Control 


vehicle  modes  of  behavior.   At  the  Sensing  level,  all 
information  concerning  the  environment  surrounding  the 
vehicle,  as  well  as  its  own  internal  state  of  health,  is 
directed  to  the  higher  level.   Figure  1.1,  reproduced  here 
from  [Ref.  2]  illustrates  the  idea,  and  Figure  1.2 
illustrates  the  hierarchical  nature  of  the  intelligent 
controls  required. 

Part  of  the  sensing  and  reflexive  acting  at  the  lowest 
level  involves  a  high  degree  of  servo-control  over  all  six 
degrees  of  freedom  of  the  vehicle  motion.   To  effect  proper 
control,  not  only  must  the  autopilot  be  capable  of  accurate 
course  and  depth  control,  but  also,  commands  for  reflexive 
actions  for  avoidance  or  attack  must  be  followed  accurately, 
These  commands  can  also  include  hovering  while  some  form  of 
underwater  work  is  done. 

B.   AIM  OF  THE  STUDY 

This  thesis  is  concerned  with  the  lowest  level  of 
control — the  control  of  vehicle  reflexive  maneuvers.   It  is 
assumed  that  the  planning  level  control  in  Figure  1.2 
recognizes  the  need  for  evasive  action  and  decides  on 
parameters  such  as  speed,  course,  and  depth  changes  to  be 
rapidly  implemented.   These  parameters  are  then  fed  to  a 
series  of  stored  maneuvers  within  the  framework  of  a  model 
based  autopilot  system.   Figure  1.3  illustrates  the  concept 
of  the  "bag"  of  maneuvers  as  interfaced  to  the  vehicle 
autopilot.   The  control  concept  proposed  here  is  that  of  a 
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Figure  1.2   Hierarchical  Control  Structure 
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Figure  1.3   Schematic  of  Supervisory  Type  Control  Scheme 


developed  for  such  maneuvers  can  be  in  the  form  of 
algorithms  that  provide  a  command  generation  system  to  the 
autopilot. 

The  purpose  of  this  work  is  to  determine  the  feasibility 
and  the  autopilot  design  methodology  for: 

1.  the  command  generation  logic,  and 

2.  a  model  following  autopilot  control. 

C.   METHOD  OF  APPROACH 

Since  this  work  deals  strongly  with  underwater  vehicle 
dynamics  and  control,  but  not  with  the  vehicle  hydrodynamics 
per  se,  it  was  important  to  use  an  existing  vehicle  model  as 
the  basis  for  the  work.   Such  a  model  (Figure  1.4)  was 
provided  by  [Ref.  4]  where  the  verification  of  the  model  by 
experimental  data  illustrated  the  reasonableness  of  its 
coefficients. 

Using  the  equations  of  motions  of  the  vehicle,  the 
development  of  command  generation  logic,  the  design  of  the 
model  following  autopilot,  and  the  AUV  maneuvering 
performance,  have  been  accomplished  with  computer 
simulation.   Heavy  use  of  the  DSL  (Dynamic  Simulation 
Language)  has  been  made.   [Ref.  5] 

The  vehicle  selected  as  the  basis  for  the  study  is 
approximately  17  feet  long  and  has  been  simulated  over  a 
range  of  speeds  from  3  to  30  feet  per  second  where  a 
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specific  maneuver — a  rapid  dive  to  100  feet — has  been  the 
focus  for  the  command  generation  model. 

While  much  remains  to  be  done,  the  concept  proposed 
appears  worthy  of  future  work. 


II.   VEHICLE  DYNAMIC  MODELING 

A.  GENERAL 

This  chapter  describes  the  dynamics  of  a  selected  AUV. 
The  three  dimensional  motion  of  an  underwater  vehicle  is 
fully  defined  using  two  coordinate  reference  systems. 

1.  Body  Fixed  Coordinate  Reference  System — Figure  2.1. 

2.  Inertial  Reference  System — Figure  2.2. 

The  vehicle  equations  of  motion  are  presented  and  how 
they  were  modified  to  suit  the  needs  of  an  AUV.   Also 
included  as  part  of  this  chapter  is  a  description  of  the 
derivation  of  the  hydrodynamic  coefficients  and  a  brief 
discussion  of  the  propulsion  plant  and  crossflow  drag 
modeling. 

B.  COORDINATE  SYSTEMS 

Three  dimensional  motions  of  underwater  vehicles  are 
normally  described  using  two  coordinate  reference  frames. 
The  first  is  a  right-handed  orthogonal  coordinate  system 
fixed  in  the  body.   The  second,  an  inertial  reference  frame, 
is  used  to  define  translational  and  rotational  motions  in 
global  coordinates  (Figure  2.1) 

The  body  fixed  coordinate  reference  frame  has  its  origin 
fixed  to  the  body  center  and  is  aligned  with  the  body  axis 
of  symmetry.   Components  of  the  vehicle  motion  relative  to 
this  body  fixed  reference  frame  are  defined  as: 
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u,v,w    components  along  the  body  fixed  axes  of  the 
velocity  of  the  origin  relative  to  the  fluid 
(Surge,  Sway  and  Heave  velocity  respectively) . 

p,q,r    components  along  the  body  fixed  axes  of  the 
angular  velocity  of  body  relative  to  the 
inertial  reference  system  (Roll,  Pitch,  and  Yaw 
rates)  (Figure  2.2). 

The  inertial  reference  frame  is  also  a  right  handed 
orthogonal  coordinate  system  in  which  the  position  and 
orientation  of  the  vehicle's  coordinate  system  is  specified. 
The  orientation  of  the  body-fixed  coordinate  system  is 
described  by  Euler  angles  ip  (yaw)  ,   0   (pitch)  ,  <j>  (roll)  . 
The  transformation  from  body-fixed  to  inertial  is  then  given 
conveniently  by  an  XYZ  rotation  sequence  (cj>,0,ijj). 

Position  of  the  body-fixed  coordinate  system  is  then 
expressed  in  X,  Y,  and  Z  coordinates  as  illustrated  in 
Figure  2.1.   Orientation  of  the  vehicle's  coordinate  system 
is  expressed  in  Euler  angles  $,    e,  ^. 

C.   RIGID  BODY  DYNAMICS  AND  EQUATIONS  OF  MOTION 

The  equations  of  motion  for  a  six  degree  of  freedom 
submarine  vehicle  are  now  standardized  being  first  fully 
developed  by  Gertler  and  Hagen  [Ref .  6] .   These  equations 
are  commonly  known  today  as  the  DTNSRDC  2510  equations  of 
motion. 

Modifications  to  these  standard  equations  are  then 
generally  made  to  reflect  the  particular  hydrodynamic 
characteristics  and  properties  of  the  underwater  vehicle 
being  considered.   [Ref.  6]   Among  the  most  significant 
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changes/allowances  considered  for  the  AUV  in  this  study 
included  an  integral  formulation  of  the  viscous  crossflow 
forces  and  moments,  addition  of  the  effect  of  an  external 
current  and  perhaps  the  most  significant  difference  is  the 
change  made  due  to  the  non-conventional  shape  of  the  AUV. 
The  AUV  considered  here  is  peculiar  in  that  its  shape  is 
more  of  low  aspect  ratio  wing  than  that  of  the  conventional 
body  of  revolution.   [Ref.  4]   Additional  modifications  were 
also  made  by  the  separation  of  the  coupled  input  for  bow  and 
stern  planes  and  also  the  decoupling  of  the  bow  planes  so 
that  purposely  induced  roll  control  could  be  included. 

The  equations  of  motion  for  the  six  degree  of  freedom 
AUV  are  listed  in  Appendix  A,  in  the  following  form: 

M  x  =  f(x,z,u)  (2.1) 

where, 

M  =  MASS  MATRIX  (2.2) 


x  =  [u,v,w,p,q,r]T  (2.3) 

f  =  [Fx,Fy,Fz,K,M,N]T  (2.4) 

and  Fx,  Fy,  Fz  are  hydrodynamic  forces  and  K,  M,  N  are 
hydrodynamic  moments, 
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X  = 


z  = 


u  = 


u 

surge 

V 

sway 

w 

=  -   heave 

=   Body  Coordinate  States     (2.5) 

p 

roll 

q 

pitch 

r 

yaw 

X 

Y 

Position 

Z 

(2.6) 

♦ 

-  Inertial  Reference  System 

0 

Orientation- 

* 

°r 
Sbs 
^p 
Ss 

Srm 


rudder  angle 

ST&D  bow  plane  angle 

port  bow  plane  angle 

stern  plane  angle 

delta  form 

delta  buoyancy 


(2.7) 


control 
vector 


and  u  is  distinguished  by  context  from  u — surge  velocity  of 
the  vehicle  relative  to  the  surrounding  water,  or  Uco  for 
the  current. 

In  addition  to  the  six  equations  of  motion  that  define 
the  AUV's  motion  relative  to  the  body  fixed  coordinate 
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system,  six  additional  equations  are  required  to  fully 
specify  the  vehicle's  motion  in  space.   These  kinematic 
relations  (see  Appendix  A)  specify  the  position  and 
orientation  of  the  body  coordinates  with  respect  to  an 
inertial  reference  frame  as  established  by  the  XYZ 
rotations,  and  are  expressed  in  terms  of  linear  velocities 
and  Euler  angular  rates. 

D.   HYDRODYNAMIC  COEFFICIENTS 

Although  development  of  the  hydrodynamic  coefficients  is 
not  a  thrust  of  this  thesis,  a  brief  description  of  their 
derivation  is  warranted. 

The  hydrodynamic  coefficients  provide  the  source  of  the 
behavioral  characteristics,  and  thus  the  responsiveness,  of 
a  particular  underwater  vehicle. 

These  coefficients  are  the  result  of  a  Taylor  series 
expansion,  in  which  only  the  first  order  terms  are  saved, 
based  on  the  motion  variables  of  the  hydrodynamic  forces  and 
moments.   The  hydrodynamic  coefficients  are  non- 
dimensionalized  and  can  be  considered  constants  within 
limited  operating  ranges.   [Ref.  6] 

There  are  currently  two  primary  methods  utilized  for 
obtaining  hydrodynamic  coefficients.   The  first  is  based  on 
tow  tank  experiments  using  planar  motion,  and  rotating  arm 
mechanisms.   The  second  is  a  geometric  analytical  approach 
using  semi-empirical  techniques.   [Ref.  4] 
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The  coefficients  used  for  this  thesis  are  those  that 
were  determined  using  the  analytic  approach  for  an  SDV 
simulator.   [Ref.  4] 

The  coefficients  thus  selected  were  chosen  because  of 
convenience  and  availability  rather  than  any  particular 
desirability  of  the  hydrodynamic  characteristics  implied. 

E.   PROPULSION  AND  CROSSFLOW  DRAG  MODELING 

1.  Propulsion  Plant  Modeling 

In  NCSC's  report  by  Crane,  Summey  and  Smith  [Ref. 
4],  propulsion  plant  modeling  is  discussed.   In  that  report 
they  list  the  effects  of  propulsion  on  the  motion  of  a 
submersible. 

propulsion  thrust 

propeller  slipstream  effects 

propulsive  torque 

propulsion  induced  hull  effects 
Of  these  four  effects  only  the  first  two  are  considered 
substantial  and  the  last  two  are  considered  negligible. 

The  propulsive  thrust  equation  was  derived  by  NCSC 
by  curve  fitting  experimental  data  and  the  propeller 
slipstream  effects  are  modeled  as  a  function  of  vehicle 
speed,  propeller  rpm,  and  geometry.   [Ref.  4] 

2 .  Crossflow  Drag  Modeling 

Since  the  AUV  geometry  selected  in  this  study  is 
essentially  a  low  aspect  ratio  wing  design  and  not  a  body  of 
revolution,  its  body  cross-sections  are  nearly  rectangular 
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rather  than  circular.   Because  of  this  an  integral  strip 
theory  formulation  of  crossflow  forces  and  moments  was 
developed  and  incorporated  into  the  equations  of  motion  as 
given  in  Appendix  A. 

F.   BOW  PLANE  INFLUENCE 

Bow  plane  action  serves  to  augment  stern  plane  control 
over  pitch  motions,  but  adds  to  the  hydrodynamically  induced 
drag  on  the  vehicle.   When  port  and  starboard  bow  planes  are 
separately  controlled,  active  control  over  vehicle  roll 
motion  may  be  accomplished.   Thus  the  coefficients  relating 
to  the  heave  and  pitch  motions,  axial  drag,  and  roll  motions 
have  been  modified  here  to  allow  separate  active  roll 
control . 
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III.   LINEARIZATION  OF  THE  VEHICLE  EQUATIONS  OF  MOTION 

A.  GENERAL 

The  overall  objective  of  this  chapter  is  to  fully 
describe  the  techniques  used  to  linearize  the  highly  non- 
linear equations  of  motion.  A  step  by  step  and  term  by  term 
development  of  the  linearized  equations  are  presented  and 
all  variables  are  completely  specified  in  their  relation  to 
the  AUV  in  this  study. 

A  description  of  the  linearization  point  and  the 
ramifications  of  linearization  about  a  straight  line  path  is 
also  considered  in  this  section. 

B.  LINEARIZATION  PROCEDURES 

Linearization  of  the  vehicle  dynamics  is  required  for 
the  design  of  the  vehicle  control  system.  The  linearized 
equations  also  serve  as  the  model  reference  for  the 
controller.  The  desired  form  is  the  state  space 
representation  of  the  equations  of  motion  given  as, 

M  AX  =  A  Ax  +  B  Au  (3.1) 

As  discussed  in  Chapter  II,  the  vehicle  dynamics  are 
represented  in  the  following  form: 

M  x  =  f(x,z,u)  (3.2) 
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where  M  is  the  mass  matrix,  x  is  the  time  derivative  of  the 
state  vector  x  and  u  is  the  input  vector.  For  the  immediate 
purpose  at  hand,  z.  may  be  considered  to  be  part  of  the  state 
vector  x.  Proper  separation  will  be  discussed  in  what 
follows. 

Linearization  is  accomplished  by  a  Taylor  series 
expansion  about  a  nominal  path  or  trajectory  given  generally 
by  (Xo(^) /Moft)) »  with  only  the  first  order  terms  being 
retained.   The  following  form  is  then  obtained: 

3f  (x  ,u  )Ax 
M  x0  +  MAX  =  f(x0,u0)  +   ~^°  = 


3f  (xvuJAu 
~8u~ 


+  ___o-o__  {33) 


where,   if  ax  =  (x  -  x0)  ,   and  AY  =  (y  ~  U0)  ,      and  Equation 
(3.3)  becomes, 

3f(x  ,u  )Ax   3f(x  ,u  )Au 

Hax=    "V  "+  "Tu    "  '     (3-4) 

Defining  A  =  ^ and  B  =   g- the  desired 

state  space  form  is  obtained. 

C.   APPLICATION  TO  VEHICLE  MODEL 

The  state  space  model  is  a  12  state  model  that  can  be 
separated  into  two  state  vectors  x  and  z.  The  state  vector 
x  represents  the  three  linear  velocities  and  corresponding 
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three  angular  rates  about  an  orthogonal  coordinate  system 
fixed  in  the  body  as  defined  in  Equation  (2.5). 

The  state  vector  z.  represents  the  six  kinematic 
relations,  three  coordinate  positions  and  three  Euler  angles 
and  is  defined  in  Equation  (2.6). 

The  two  sets  of  six  equations  that  result  are  of  the 
form: 


x  =  M_1f  (x,z,u) 


(3.5) 


z  =  g(x,z) 


(3.6) 


The  control  vector  u  is  the  input  vector  and  is  defined 
by  Equation  (2.7). 

By  combining  both  state  vectors,  the  model  state  vector 
is  defined, 


X  =    [x,z]T   =    [uvwpqrXYZ<j)0  ty]T 


(3.7) 


Once  the  model  state  vector  and  control  vector  are 
defined,  the  A  matrix  and  B  matrix  must  be  determined.  The 
A  matrix  formulation  is  represented, 


A 
12  x  12 


dx 


dx 


m'^vvV 


8z 


dz 


(3.8) 
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and  by  similar  formulation  the  B  matrix  is, 


B 
12  x  6 


m'^V^ 

•%> 

3^o 

3Ho 

(3.9) 


An  element  by  element  formulation  of  the  A  and  B 
matrices  are  complex  and  require  careful  attention.  The 
particular  functional  form  of  the  derivative  expressions 
can,  however,  be  obtained  analytically  and  depending  on 
whether  x0  and  zQ  are  time  dependent  or  constant,  the 
analytical  derivatives  become  time  variant  or  not.  For  the 
case  of  linearization  about  a  straight  line  flight  path, 
these  derivatives  are  constant  which  makes  the  control 
computations  easier  than  for  more  complicated  nominal  flight 
conditions. 

D.   LINEARIZED  VARIABLES  ABOUT  STRAIGHT  FLIGHT  PATHS 

One  convenient  feature  concerning  the  linearization 
about  a  straight  flight  path  with  forward  speed,  u0,  is  that 
A  and  B  become  constant  matrices  where  the  coefficients  are 
relatively  simple  functions  of  the  forward  speed.  Also, 
since  the  nominal  path  is  associated  with  neither  rotation 
nor  cross-track  or  depth  translation,  the  incremental 
variables  Ax  and  Au  are  identical  to  the  actual  variables  x 
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and  u  except  for  the  longitudinal  velocity  and  position. 
The  linearized  dynamics  in  the  axial  direction  become: 


at*  =  %  (3-io) 


so  that  as  far  as  the  linearized  system  dynamics  are 
concerned  Ax(l)  =  0  and  Ax(t)  =  u0(t) .  While  this  feature 
is  convenient,  it  does  not  provide  information  on  the  second 
order  effect  of  control  surface  action  slowing  down  the 
vehicle. 

A  possible  approach  to  alleviating  this  deficiency  in 
the  linear  model  could  be  to  modify  the  axial  direction 

equation  of  motion  so  that  the  drag  effects  of  control 

2 
surface  action  are  related  to  | 62 1  rather  than  6S.   This  is 

beyond  the  scope  of  this  thesis. 
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IV.   AUTOPILOT  DESIGN  USING  OPTIMAL 
CONTROL  TECHNIQUES 

A.  GENERAL 

This  chapter  contains  a  review  of  optimal  control 
techniques  as  developed  and  used  in  this  study  for  the 
control  of  autonomous  underwater  vehicles.  Such  an 
autopilot  has  been  classically  treated  as  a  series  of 
interconnected  feedback  loops  for  independent  control  of 
depth  and  control  of  course  and  heading,  while  roll  control 
of  the  vehicle  has  been  left  passive.  Control  of  the  sixth 
degree  of  freedom,  longitudinal  velocity,  has  not  been 
considered  important  and  a  constant  thrust  or  propeller 
speed  has  been  assumed. 

While  control  of  all  six  degrees  of  freedom  may  be 
important  in  the  end  for  future  AUV  operations,  and 
particularly  in  the  transition  from  cruise  to  hover  modes, 
this  is  not  the  primary  focus  here.  Instead,  this  chapter 
deals  with  the  state  of  the  art  in  systems  concepts  for 
underwater  vehicle  course  and  depth  control,  together  with  a 
review  of  the  modern  multivariable  system  controls  methods 
used  in  modern  autopilot  design. 

B.  CLASSICAL  CONTROL  OF  COURSE  AND  DEPTH 

Simple  autopilots  have  long  been  of  interest  in 
relieving  the  human  operator  of  onerous  tasks  and  preventing 
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fatigue.  Classical  design  techniques  have  considered  depth 
and  course  control  as  separate,  non-interacting  control 
systems.  The  depth  controller  directs  commands  to  the  stern 
planes  based  on  an  error  between  pitch  angle  command  and 
vehicle  pitch  angle  where  the  pitch  command  is  proportional 
to  depth  error.  Course  heading  controllers  provide  rudder 
angle  commands  proportional  to  heading  angle  error.  Walker 
[Ref.  7]  recently  proposed  the  addition  of  a  cross  track 
position  feedback  loop  using  yaw  angle  damping  to  control 
the  cross  track  distance  for  automatic  track  control. 

Most  vehicle  controllers  in  practice  rely  on  classical 
concepts  with  protection  limits  on  command  signals  so  that 
control  surface  commands  can  be  limited  in  magnitude  and 
rate.  Adaptive  steering  controllers  have  been  proposed  as 
an  extension  for  course  maintenance  in  heavy  seas  when 
optimized  gain  settings  are  based  on  calm  weather  ship 
characteristics  [Ref.  8].  The  main  limitation  of  autopilot 
designs  based  on  classical  concepts  are, 

1.  Ship  characteristics  vary  strongly  with  speed  so  that 
gain  settings  for  all  of  the  major  loops  have  to  be 
adjusted  to  maintain  optimum  performance  under  wide 
operating  conditions. 

2.  Gains  set  based  on  maximum  actuation  limits  and 
usually  designed  to  regulate  vehicle  depth  and  course 
about  nominally  fixed  reference  settings. 

3.  Control  of  depth  and  course  changes  (i.e.,  rapid 
maneuvering)  is  not  easy  and  usually  not  automated. 

The  control  of  rapid  maneuvering  is  more  suited  to  the 

more  recent  multivariable  control  system  structures  such  as 
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those  involving  Model  Following  Controllers,  and  Model  Based 
Compensators  as  proposed  by  Milliken  [Ref.  9]. 

C.   REVIEW  OF  OPTIMAL  CONTROL  CONCEPTS  RELATING  TO 
CONTROLLER  DESIGN 

1.   LOR  Summary  and  Discussion 

Much  has  been  written  about  the  application  of 

Optimal  Control  Concepts  to  the  design  of  feedback  systems 

for  both  output  regulation  and  input  tracking.   Kwaakernaak 

and  Sivan  [Ref.  10]  present  a  discussion  of  design  methods 

based  on  state  of  the  art  to  1971.   Kaufman  and  Berry  [Ref. 

11]  have  provided  examples  of  autopilot  design  methods  based 

on   linear   optimal   regulator   (LQR)   methods   and   model 

following   techniques.     Milliken   [Ref.   9]   has   showed 

recently,  the  use  of  Model  Based  Compensators  for  providing 

multi-degrees  of  freedom  control  for  a  submarine  depth  and 

course  control  using  linear  control  techniques — similar  to 

those  used  in  this  work.   Most  recently,  non-linear  control 

methods  have  been  proposed  by  Slotine  [Ref.  12],  and  Yoeger 

and  Slotine  [Ref.  13]  to  provide  robust  trajectory  control 

for  underwater  vehicles.   Using  linear  control  procedures, 

the  vehicle,  or  object  to  be  controlled  is  described  by  a 

linear  state  variable  dynamic  model  for  response  computation 

by  equations  of  the  form, 

xp(t)  =  Ap  xp(t)  +  Bp  up(t);    xp(0)  given        (4.1) 
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in  which  the  matrices  Ap  and  Bp  represent  constant 
coefficient  terms,  and  xp(t)  and  up(t)  respectively, 
represent  the  vector  of  motions  (positions  and  velocities) 
and  the  control  actions  (control  surface  deflections) . 

The  design  of  a  linear  optimal  regulator  (LQR) 
control  is  based  on  the  notion  that  if  some  non-zero  initial 
condition,  x(0) ,  is  established,  then  up(t)  can  be  designed 
so  that  the  non-zero  state  values  can  be  reduced  to  the 
equilibrium  values  x(t)  =  0,  x(t)  =  0  with  a  control 
operation  given  by, 

Up(t)  =  -  K  xp(t)  (4.2) 

where  K  is  found  from  the  minimization  of  the  quadratic 
performance  index, 


J  =   /   (xT  0.   x  +  uT  R  u)  dt  (4.3) 

0 


Here,  Q,  is  a  non-negative  definite  square  symmetric 
weighting  matrix  for  response  magnitudes  and  R  is  a  positive 
definite  square  symmetric  weighting  matrix  for  control 
effort.  Q  is  size  nxn,  and  R  has  rank  equal  to  the  number 
of  control  inputs  modeled  (r) . 

The  solution  for  K  becomes  a  matrix  of  size  rxn 
found  as  the  solution  to, 
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K  =  R""1  BT  P 


(4.4) 


where 


P  A  +  ATP  +  Q  -   P(B"1K"1BT)  P  =  0 


(4.5) 


The  eigenvalues  of  the  closed  loop  regulator  are 
determined  from  the  combined  state  and  co-state  system 
equations.  They  are  given  by  the  eigenvalues  of  the 
composite  matrix  SS  where, 


SS   = 


-Q 


B  R"1  BT 


-aT 


(4.6) 


It  can  be  show  [Ref.  10]  that  P  is  also  given  by, 


P  =  [W2]  [WX] 


-1 


(4.7) 


where  W^,  W2  are  the  nxn  partitions  of  the  matrix,  W, 


W  = 


W- 


W2 


(4.8) 


formed  from  columns  of  stable  eigenvectors  of  SS.   It  has 
also  been  found  that  the  use  of  real  part  and  imaginary 
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parts  of  a  complex  conjugate  eigenvector  as  adjacent  columns 
of  W  where  a  complex  pole  pair  exist,  eliminates  the  need 
for  complex  matrix  inversion  [Ref .  10] . 

The  design  by  minimization  of  J  in  Equation  (4.3) 
yields  the  closed  loop  control  system  equations, 

Xp(t)  =  (A  -  B  K)Xp(t);    Up  =  -K  xp;    xp(0)  given    (4.9) 

where  the  steady  state  response  is  zero  for  both  Xp  and  Up. 

The  state  vector  may,  in  many  cases,  be  considered 
as  a  deviation  vector  from  a  desired  constant  level,  and  it 
is  quite  appropriate  for  the  steady  state  values  of  Xp  and 
up  to  go  to  zero.  However,  in  the  reality  of  some  cases, 
the  maintenance  of  a  constant  level  in  some  elements  of  the 
state  vector  requires  a  non-zero  steady  state  control  signal 
level  and  in  these  cases  Up(°°)  =0.  If  a  steady  state  level 
must  be  maintained  for  any  element  of  the  state  vector,  the 
steady  state  equations  are  first  solved  as, 

0  =  Ap  Xp(oo)  +  Bp  Up(oo)  (4.10) 

Equation  (4.10),  subtracted  from  Equation  (4.1) 
reduces  these  cases  to  the  equivalent  of  a  regulator  control 
problem  by  shift  of  variable, 

xp(t)  =  Ap(xp(t)  -  Xp(oo))  +  Bp(up(t)  -  Up(oo))  (4.11) 
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where  the  new  variables 


xD(t)  -  xD(oo)  (4.12) 


and 


uD(t)  -  uD(oo)  (4.13) 


are  related  in  a  control  law 


uD(t)  -  uD(co)  =  -K(xD(t)  -  &(»))  (4.14) 


or 


Un(t)  =  UD(oo)  -  K(XD(t)  -XD(oo))  (4.15) 


The  above  discussion  has  been  limited  to 
deterministic  signals  and  to  the  assumption  '  that  all 
physical  state  variables  are  either  measurable  or  determined 
in  a  full  state  observer  [Ref .  10] . 

Where  the  output  of  the  controlled  process  is  to  be 
regulated,  the  above  techniques  may  be  used  to  design  the 
elements  of  the  feedback  gain  matrix  thus  avoiding  the 
complex  task  of  designing  separate  control  loops  from  each 
variable  in  the  process.    The  method  is  powerful,  but 
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requires  skill  in  the  selection  of  appropriate  £  and  R 
factors. 

2.   Tracking  Control  Systems — (LMFC  +  MRAC) 

Where  the  control  system  is  required  to  drive  a 
process  so  that  the  output  tracks  an  input  variable  within 
acceptable  error  bounds,  the  problem  is  further  compounded. 
Even  more  difficult  is  to  achieve  the  tracking  of  several 
simultaneous  inputs  by  the  various  outputs  of  the  driven 
process.  During  the  late  1960 's  and  early  1970 's,  much 
attention  was  placed  on  linear  model  following  controls 
(LMFC)  and  model  reference  adaptive  controls  (MRAC)  to 
provide  the  acceptable  tracking  behavior  of  multivariable 
systems.  Kaufman  and  Berry  [Ref.  11]  described  the 
application  to  flight  control,  and  Landau  [Refs.  14,15]  gave 
a  survey  of  design  techniques  and  system  structures  in  which 
it  became  clear  that  a  model  of  the  system  to  be  controlled 
was  needed  to  represent  the  desired  time  behavior  of  the 
system  state  variables.  The  system  control  variables  then 
became  a  function  of  the  input  variables  to  the  model,  and 
the  model  state  variables,  in  addition  to  the  feedback  of 
system  state  variables.  Thus  better  information  than  could 
be  derived  by  feedback  was  used  to  drive  outputs  to  track 
inputs. 

The  use  of  MRAC  techniques  allows  for  not  only  model 
following,  but  also  the  provision  of  adapting  gains,  or 
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model  parameters  in  achieving  precise  control  when  system 
operating  conditions  change. 

One  of  the  difficulties  pointed  out  by  Landau  [Ref. 
14],  is  that  controller  parameters  need  to  change  when  the 
plant  operating  conditions  changed.  Thus  using  a  reference 
model  not  only  provides  the  robustness  achieved  by 
predictive  and  corrective  control  but  also  provides  the 
opportunity  to  update  model  and  control  parameters 
automatically. 

Restricting  the  discussion  to  Linear  Model  Following 
Controls  (LMFC) ,  the  control  issues  are  analyzed  as  follows: 
the  plant  model  is  given  by, 

xp  =  Ap  xp  +  Bp  Up  (4.16) 

Yp  =  %  xp  (4.17) 

and  a  suitable  model  of  the  plant,  but  with  '  desirable 
dynamic  response  characteristics  (response  time,  stability, 
etc.)  is  given  by, 

x-m  =  Am  xm  +  Sm  um(t)  (4.18) 

Um  =  0  (4.19) 

Ym  =  Cm  xm  (4.20) 
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then  the  control  signals,  Up,  which  minimize  the  weighted 
integral  of  errors  between  model  and  plant  are  given  by, 

Up  =  Ki  uffl  +  £2  xm  +  K3  *p  (4.21) 

where  the  errors  are  defined  as, 

e  =  Y-m  -  *p  (4-22) 

and  the  performance  index  minimized  is, 


J  =  /  (LT  0.  L     +  MnT  E  uD)  dt  (4.23) 

0  P  P 


and  Q,    and  R  are  weighting  matrices  as  discussed  earlier. 

The  computation  of  the  gain  matrices,  K-^,  K2 ,  and  K3 
are  fortunately  made  easier  by  considering  the  combined 
system,  model  plus  plant  as  a  coupled  linear  system.  Also, 
to  overcome  problems  that  arise  when  the  signals  to  be 
tracked,  Ym(t)  are  derived  from  inputs,  um(t) ,  that  are  not 
impulses,  it  is  convenient  to  consider  that  the  additional 
model  equations, 

Mm  =  °  (4.24) 

be   incorporated  together  with  um  as  a  composite  state 
vector, 
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x   -  [Xp  /Xm  /Um  ] 


(4.25) 


to  get  the  system  equations, 


d_ 
dt 


xp 

x.m 


Ap 


0 


0 


im  Sm 
0     0    0 


xp 

X-m 

+ 

0 

Mm 

0 

H 


(4.26) 


Now,   application  of  the  LQR  technique  to  the  composite 
system  given  above  yields, 


Up  =  -[R"1  BpT][PHxp  xm  um]T 


(4.27) 


where.  P  now  is  of  dimension  (np  +  nm  +  rm) ,  and  [R  ^   BT  P] 
is  partitioned  in  three  parts, 


[R-1  BT  P]  =  [K3    K2    KX] 


(4.28) 


By  varying  the  weighting  factors  within  the  matrix, 
Q,  selected  errors  may  be  penalized  more  heavily  than  others 
in  the  optimal  control  trade-off.  Also,  selection  of 
parameters  within  R  may  be  used  to  provide  a  trade-off 
between  a  sluggish  or  sensitive  control  design.  Details  of 
numerical  values  used  in  the  design  of  the  autopilot 
controls  are  given  later  in  Chapter  VI. 
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3.   Near  Time  Optimal  Maneuvering  Models 

The  use  of  the  system  structure  implied  by  Equation 
(4.26)  and  the  resulting  control  law,  Equation  (4.21),  is 
particularly  useful  when  considering  near  time  optimal 
positioning  of  inertial  objects.  It  is  well  known  that  time 
optimal  position  control  of  a  massive  object  requires  a 
bang-bang  application  of  force  or  torque.  These  concepts 
are  recently  being  considered  in  robot  tracking  control 
[Ref.  16],  and  the  sliding  control  described  in  [Ref.  17]. 
So  also,  in  the  field  of  LMFC  for  underwater  vehicle 
maneuvering  control,  it  is  expected  that  rapid  maneuvering 
will  require  some  form  of  bang-bang  operation  of  control 
surfaces.  Bang-bang  operation,  in  principle,  is  simple, 
consisting  of  a  sequence  of  stepwise  control  actions,  yet 
knowledge  of  switching  times  for  anything  other  than  very 
low  order  systems  make  the  principle  difficult  to  implement. 
The  outcome  of  the  above  discussion  then  leads  to  the 
development  of  vehicle  maneuvering  models  based  on  use  of  a 
series  of  constant  setting  for  control  surfaces  that  make  up 
the  model  input  vector  um.  At  times  during  the  response  of 
the  model  where  switching  should  occur,  the  control  surfaces 
change  setting  rapidly  as  if  by  imposition  of  an  impulse 
command.  Therefore,  if  it  is  considered  that  surface 
settings  change  levels  at  discrete  but  arbitrary  times,  the 
unforced  nature  of  the  model  reference  states,  in  Equation 
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(4.26)  are  preserved  and  the  application  of  the  LMFC  system 
is  valid. 

For  every  reflexive  maneuver  envisioned  during  the 
operation  of  an  AUV  life,  it  is  foreseen  that  maneuvering 
logic  can  be  developed  on  an  algorithmic  basis  to  determine 
switching  times,  using  logic  to  be  developed  and  the  Am,  B^, 
Ki,  K2  and  K3  matrices  as  shown  in  Figure  4.1.  These  data 
can  be  stored  inside  on-board  processors  so  that  on  command 
from  the  high  level  controller  or  expert  system,  new 
computations  for  up  can  be  implemented  immediately. 

The  development  of  a  maneuvering  logic  as  a  command 
generation  system  for  a  dive  maneuver  will  be  discussed  in 
more  detail. 
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V.   SIMULATION  TECHNIQUES 

A.  GENERAL 

This  chapter  contains  a  discussion  of  the  computational 
program  structures  used  in  this  study.  All  simulations  were 
performed  using  the  Dynamic  Simulation  Language  (DSL)  [Ref. 
5]  code  for  the  simulation  of  linear  and  non-linear  system 
response  as  a  function  of  time.  Internal  numerical 
integration  routines  make  this  aspect  of  the  solution 
transparent  to  the  user.  The  user  provides  only  the  details 
of  the  particular  equations  employed.  In  this  work,  DSL  was 
used  for  the  simulation  of  both  uncontrolled  and  autopilot 
controlled  vehicle  responses.  However,  as  part  of  the 
design  procedure  for  the  autopilot,  the  complete  set  of 
feedforward  and  feedback  gains  were  established  using  ETAT — 
a  specially  developed  program  for  the  computation  of  linear 
optimal  control  gains.  The  pertinent  linkages  between  DSL 
and  ETAT  were  developed  and  implemented  during  this  study. 
More  detailed  descriptions  follow. 

B.  COMPUTATION  OF  FEEDFORWARD  AND  FEEDBACK  GAINS 

While  the  theory  behind  the  need  for  feedforward  gains 
for  optimal  model  following  autopilots  has  been  given  in 
Chapter  IV,  this  section  discusses  the  program  organization 
used  in  their  computation. 
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The  outline  organization  of  program  ETAT  is  shown  in 
Figure  5.1.  ETAT  reads  and  writes  values  of  AA,  and  BB,  as 
computed  within  the  framework  of  the  DSL  simulation  and  also 
reads  user  input  values  for  the  tracking  error  weighting 
matrix,  Q,  and  the  control  input  weighting  matrix,  R. 
Particular  values  used  for  Q,  and-  R,  are  given  later  in 
Chapter  VI. 

Subroutine  MTXEXP  computes  the  matrix  exponential 
associated  with  AA,  and  the  discrete  time  input  matrix 
associated  with  AA  and  BB,  but  this  section  has  not  been 
used  here. 

Subroutine  ROOTS  is  used  for  the  computation  of  both 
eigenvalues  and  eigenvectors  of  a  square  matrix  (AA) ,  and 
calls  the  IMSL  double  precision  library  routine  EIGRF  and 
its  associated  subroutines. 

OPTIMA  is  the  subroutine  used  for  assembly  of  the 
composite  state  and  co-state  matrix,  SS.  OPTIMA  also  calls 
EIGRF  and  computes  the  closed  loop  system  eigenvalues  and 
vectors.  These,  as  given  earlier  in  Chapter  IV,  are  used  to 
form  the  solution  of  the  matrix  Ricatti  equation  and  the 
overall  matrix  of  gains,  i.e.,  Equation  (4.4).  Partitions 
of  the  overall  gain  matrix  give  the  individual  matrices,  K^, 
K2  and  K3  in  Equation  (4.28). 

A  listing  of  the  major  subroutines  used  in  program  ETAT 
are  provided  in  the  appendix  for  the  interested  reader, 
although  use  of  ETAT  without. proper  linkages  to  DSL  and   the 
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Figure  5.1   ETAT  Flow  Diagram 
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appropriate   IMSL  double  precision  library  would  not  be 
proper. 

C.   REFERENCE  MODEL  DEVELOPMENT 

As  discussed  earlier,  the  reference  model  is  a  full  12 
state  representation  of  the  AUV.  The  reference  model  can  be 
represented  by: 

xm  =  AA  xm  +  BB  um       um  =  0  (5.1) 

A  computational  problem  in  subroutine  OPTIMA  can  arise 
because  of  the  multiple  zero  eigenvalues  associated  with 
several  of  the  modes  in  the  above  equations.  This  problem 
has  been  overcome  here  by  inserting  very  small  values, 
-(A)i,  on  the  key  diagonal  elements  of  the  AA  matrix  so  that 
distinct  eigenvalues  result.  Since  the  (A)  values  are 
extremely  small,  their  effect  on  the  system  poles  is 
negligible  and  the  problem  of  multiple  repeated  poles  is 
eliminated. 

It  is  conceivable  to  have  a  series  of  reference  models, 
one  for  each  of  several  reflexive  maneuvers.  Each  maneuver 
will  have  its  associated  logic  that  will  generate  the 
control  input  to  the  model  and  thus  provide  the  model 
reference  states. 

For  this  thesis,  only  one  such  maneuver,  a  dive 
maneuver,  was  investigated.  Logic  for  the  dive  maneuver  is 
based  on  an  application  of  bang-bang  optimal  control  theory, 
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thereby  yielding  time  optimal  response.  The  methodology 
here  is  to  deflect  stern  planes  up  and  the  bow  planes  down 
to  initiate  a  pitch  rate  (p)  until  the  vehicle  achieves  some 
predetermined  pitch  angle  (0).  For  what  is  considered 
reflexive,  or  emergency  obstacle  avoidance,  a  large  angle  is 
desired.  Assuming  that  the  submersible  is  directionally 
stable,  some  small  stern  plane  angle  must  then  be  maintained 
to  keep  a  constant  pitch  angle,  dependent  on  speed,  until 
such  time  when  the  control  action  should  provide  an  opposite 
effect  to  come  out  of  the  dive  and  steady  at  a  new  depth. 
An  example  of  this  control  action  is  shown  in  Figure  5.2. 

Given  limits  on  control  surface  deflection  and  maximum 
pitch  angle  during  the  dive,  this  type  of  control  action 
should  provide  an  optimal  response  for  a  change  in  depth. 

With  this  control  logic  preprogrammed  into  an  AUV, 
whenever  the  supervisory  control  system  calls  for  a  dive 
maneuver,  the  logic  can  provide  the  control  input  for  the 
reference  model  and  thus  an  optimal  path  can  be  created 
quickly;  one  that  the  controller  can  track  and  vehicle  can 
follow. 

The  logic  for  the  dive  maneuver  is  crude,  however,  this 
is  a  trade-off  for  ease  in  programming  the  algorithm  used 
for  the  dive  maneuver.  When  programming  one  of  these 
reflexive  maneuvers  one  must  be  cautious  not  to  program  a 
maneuver  that  is  beyond  the  capability  of  the  vehicle. 
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Figure  5.2   Time  Optimal  Control  Action  for  a 
Dive  Maneuver  Command  Generation 
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A  conceptual  objective  is  to  have  many  maneuver 
algorithms  preprogrammed  into  a  vehicle  into  a  "bag"  of 
maneuvers.  This  bag  of  maneuvers  would  be  at  the  disposal 
of  the  supervisory  control.  This  supervisory  control  would 
be  the  manager  of  the  bag  of  maneuvers,  as  earlier  indicated 
in  Figure  1.3,  and  would  receive  its  instructions  from  the 
on-board  expert  system  or,  in  the  future,  artificial 
intelligence. 

For  the  many  types  of  standard  and  emergency  situations 
required,  collision  or  obstacle  avoidance,  a  proper  maneuver 
can  be  chosen  and  executed  quickly  and  efficiently  without 
excessive  computational  burdens  that  would  otherwise  lead  to 
a  tardy  response. 

D.   SYSTEM  SIMULATION  METHOD 

1.   Dynamic  Simulation  Language  (DSL) 

DSL  is  a  Fortran  based  simulation  language  for 
digital  simulation  of  continuous  systems.  DSL  uses  a 
building  -block  approach  to  programming.  Programs  can  be 
very  simple  or  they  can  become  extremely  complex  when  all 
the  functions  of  DSL  are  utilized.  The  user  can  enter 
Fortran  statements  in  any  order  and  DSL  can  sort  and  solve 
these  equations  effectively.  The  user  can  also  include 
fortran  subroutines  and  use  the  expansive  I/O  facility  of 
DSL.  One  other  key  feature  is  the  integration  routine 
capability.  The  user  has  the  choice  of  nine  integration 
methods;   four   fixed-step,   two   variable-step   and   three 
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variable-step,  variable  order  methods.  DSL  was  chosen 
primarily  because  it  easily  can  solve  differential  equations 
and  it  contains  many  internal  functions  that  normally  would 
have  to  be  programmed  by  the  user. 

DSL  has  four  phases  of  program  execution; 

TRANSLATION 

COMPILATION 

SIMULATION 

GRAPHICS 

DSL  translates  all  the  DSL  code  into  Fortran 
statements.  Once  the  code  is  translated,  it  is  then  linked 
to  the  VS  compiler  and  the  code  is  compiled  and  stored  as  an 
executable  file.  Upon  completion  of  the  compilation  phase, 
the  simulation  phase  begins,  and  the  system  clock  starts, 
and  simulation  continues  until  the  system  reaches  its  user 
specified  finish  time.  The  last  phase  of  problem  execution 
includes  the  graphic  capability  of  DSL.  Saved  output  data 
can  be  plotted  or  graphed  using  the  graphics  post-processor 
and  the  specific  hardware  supported. 
2 .   Problem  Simulation 

As  mentioned  earlier,  DSL  uses  a  building-block 
approach  to  programming.  The  major  blocks  and  general  flow 
of  program  simulation  are  shown  in  Figures  5.3  and  5.4.  To 
fully  understand  the  simulation,  and  the  controller  design, 
and  control  action,  a  detailed  breakdown  and  discussion  is 
required. 
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Figure  5.3   DSL  Simulation  Flow  Diagram 
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Figure  5.4   DSL  Simulation  Flow  Diagram  Continued 
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The  first  section  of  the  simulation  is  the  CONSTANT 
block.  In  this  block  all  of  the  hydrodynamic  coefficients 
and  vehicle  constants  are  read  into  the  program. 

The  second  section  is  the  INITIAL  block.  In  this 
section,  all  of  the  calculations  not  part  of  the  integration 
routines  and  those  needed  in  establishing  parameters  and 
initial  conditions  are  performed.  This  is  also  where  all 
variables  are  initialized.  The  following  calculations  occur 
in  this  section: 

1.  All  matrices  and  arrays  are  initialized  to  zero. 

2 .  The  length  and  weight  fractions  for  a  four  term  gauss 
quadrature  are  initialized. 

3.  The  breadth  and  height  terms  are  read  in.  These  terms 
will  be  used  in  the  gauss  quadrature  integration  for 
the  crossflow  drag  terms. 

4.  The  thrust  is  then  calculated  for  the  propulsion 
model . 

5.  The  non-zero  elements  of  mass  matrix  M  are  calculated. 

6.  The  square  mass  matrix  M  has  rank  of  six  is  then 
inverted  using  the  IMSL  routine  LINV2F. 

7.  The  non-zero  elements  of  the  A  matrix  are  calculated. 
These  elements  are  the  coefficients  of  the  first  order 
terms  in  the  Taylor  series  expansion  about  a  specific 
operating  point. 

8.  The  non-zero  elements  of  the  B  matrix  are  then 
calculated. 

9 .  The  next  step  is  to  multiply  the  inverse  of  the  mass 
matrix  to  obtain  the  coefficients  of  the  state 
equation,  (5.1) . 

10.  The  last  task  for  the  initial  section  is  to  read  in 
the  computed  feedforward  and  feedback  gains  from  the 
program  ETAT  that  are  to  be  used  in  the  autopilot 
control  law. 
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The  third  block  of  the  main  program  is  called  the 
DERIVATIVE  section.  Here,  all  the  first  order  equations 
that  must  be  integrated  and  solved  are  assembled.  The 
DERIVATIVE  section  of  this  simulation  is  comprised  of  three 
major  subsections,  one  for  each  major  section  in  the 
simulation. 

1.  Linear  reference  model  providing  command  generation. 

2.  Control  law  linking  model  and  vehicle  response  to 
control  surface  actions. 

3.  Nonlinear  model  for  simulating  vehicle  response. 

The  control  vector  um  is  the  input  to  the  linear 
model,  generated  from  the  maneuver  logic  contained  within 
the  DYNAMIC  section.   This  section  will  be  discussed  later. 

Once  the  control  input  is  established,  the 
derivative  expressions  of  the  linear  reference  model  are 
formulated  in  terms  of  the  matrices  AAm  and  BB^ 

After  the  linear  model  derivatives  are  established, 
the  model  states  xm  and  model  inputs  um  are  passed  to  the 
Control  Law.  The  Control  Law  (Equation  (4.21))  represents, 
in  software,  the  gains  that  would  be  incorporated  in  the 
vehicle. 

The  input  vector  up  represents  the  inputs  to  the 
actual  vehicle,  in  this  case,  defined  by  Equations  (3.2). 

The  derivatives  of  the  vehicle  state  variables  are 
formed  as  the  last  part  of  the  DERIVATIVE  section  in 
preparation  for  numerical  integration  using  the  fifth  order 
variable  step  Runge-Kutta  technique. 
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The  model  states  and  inputs,  as  well  as  the  vehicle 
states  and  inputs,  are  saved  for  graphical  representation 
and  data  output. 

The  fourth  major  block  of  the  simulation  is  the 
DYNAMIC  section.  In  the  DYNAMIC  section,  the  maneuver  logic 
is  programmed.  This  section  is  reserved  for  those 
computations  that  depend  on  time  and  are  independent  of  the 
system  responses.  However,  response  dependent  functions  may 
also  be  included  here  as  is  the  case  with  the  establishment 
of  the  reference  control  commands  generated  by  the  maneuver 
logic. 

The  fifth  section  of  the  program  is  the  CONTROL 
section.  Before  the  command  or  input  is  sent  to  the 
derivative  section,  the  system  time  clock  is  checked,  and  if 
"finish  time"  (fintime)  in  the  CONTROL  section  is  reached 
the  program  stops.  If  not,  the  system  increments  itself  one 
time  step  and  continues  with  the  simulation. 

Upon  completion  of  the  simulation  a  time  history  of 
all  desired  parameters  and  variables  are  saved  in  a  data 
file.   Plots  and  graphical  output  may  then  be  generated. 
3 .   Procedure  Used 

To  perform  a  simulated  run  with  a  particular 
autopilot  design  and  vehicle  speed,  an  initial  run  with  DSL 
was  required  to  establish  values  for  the  AA,  and  BB 
matrices.  These  values  were  written  on  as  output  files 
(file  FtlOFOOl   for  AA  and  file  Ft09F001  for  BB) .    By 


49 


separate  run,  program  ETAT  was  used  to  read  AA  and  BB,  and 
its  own  input  file  Ft05F001  for  Q  and  R  and  to  provide 
values  for  control  gains  K^,  K2  and  K3 .  The  gain  matrices, 
written  on  file  Ft02F001  were  then  read  by  a  final  run  using 
DSL  for  the  controlled  vehicle  response  simulation  and 
results  were  provided  on  data  file  OUTP. 
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VI.   RESULTS 

A.   GENERAL 

This  chapter  describes  the  efforts  and  results  of  the 
design  of  a  model  following  autopilot  for  an  AUV.  The 
controller  designed  is  only  a  partial  solution  to  the 
complete  control  over  the  six  degrees  of  freedom  of  an  AUV. 
However,  the  methodology  developed  in  this  study  could  be 
applied  to  design  a  full  30  state  controller,  12  plant 
states,  12  model  states,  and  6  control  states.  The 
controller  designed  in  this  study  is  a  19  state  controller 
using  12  plant  states,  4  model  states  relating  to  the  pitch 
plane,  wm,  qm,  zm,  6m  and  the  three  control  inputs  for  this 
plane,  port  and  starboard  bow  plane  angle  and  stern  plane 
angle. 

In  Section  D,  the  base-line  controller  is  tested  and  the 
results  show  excellent  depth  control  with  excellent  time 
history  tracking.  However,  the  control  over  pitch  angle 
appeared  loose  and  in  Sections  E  and  F  attempts  were  made  to 
gain  tighter  pitch  control. 

A  test  of  controller  robustness  is  its  ability  to 
operate  over  a  range  of  vehicle  speeds  and  changing 
parameters.  In  Section  G  the  controller  was  tested  at 
speeds  of  3,  12  and  3  0  ft/sec,  approximately  1.8,  7  and  17 
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knots,  respectively,  while  baseline  runs  were  at  6  ft/sec 
(3.5  knots) . 

Included  in  this  chapter  is  a  discussion  of  the  gains 
used,  how  they  were  derived  and  the  effects  on  the  gains  by 
varying  the  control  weight  matrix  R  and  the  control  error 
weight  matrix  0.. 

B.   RESULTS  OF  UNCONTROLLED  MANEUVERING 

The  first  simulation  runs  that  were  made  early  in  this 
study  were  to  test  the  non-linear  model.  One  maneuver  that 
was  first  tested  was  a  turning  maneuver.  Because  of  this 
AUV's  particular  geometry  (low  aspect  wing  vice  body  of 
revolution) ,  some  unique  behavioral  characteristics  are 
displayed  as  shown  in  Figures  6.1  and  6.2,  not  common  to 
other  forms  of  underwater  vehicles.  Of  the  most  significant 
is  when  a  rudder  command  is  given  the  vehicle  rolls  out  of 
the  turn.  While  this  is  not  uncommon  for  vehicles  without  a 
sail  area,  it  is  uncommon  for  a  vehicle  with  a  cruciform 
type  stern  to  dive  on  a  turn  while  the  vehicle  rolls  out. 
Although  this  vehicle's  dynamics  are  not  representative  of 
those  common  to  submersibles,  the  behavior  has  been  verified 
experimentally.  The  purpose  of  selection  was  based  purely 
on  the  availability  and  thoroughness  to  which  the  vehicle 
dynamics  were  modeled,  and  that  program  validation  was 
easily  accomplished  from  the  data  available. 
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C.   DIVE  PLANE  MANEUVER  AND  PREDICTOR  CONTROL 

Once  the  non-linear  model  was  validated  the  controller 
design  process  then  began.  The  first  simulation  of  this 
process  consisted  of  only  predictor  control,  no  feedback  was 
incorporated.  The  inputs  generated  by  the  dive  maneuver 
logic  for  stern  and  bow  plane  input  to  the  linear  model  were 
also  fed  into  the  non-linear  vehicle  dynamics. 

This  run,  Figures  6.3  to  6.7,  provided  insight  on  the 
accuracy  of  the  linearized  version  of  the  equations  of 
motion.  Figure  6.7  shows  excellent  pitch  correlation 
between  the  model  and  vehicle. 

Figures  6.4  and  6.5  both  show  that  the  vehicle  never 
reaches  its  ordered  depth  of  100  ft.  because  the  vehicle 
equations  were  linearized  about  a  straight  line  trajectory 
at  a  constant  speed,  the  linear  model  does  not  generate  any 
speed  loss  and  subsequently  the  AUV  lags  behind  the  linear 
model,  a  result  that  was  clearly  expected.  The 
responsiveness  of  the  vehicle  is  interesting  considering  the 
slow  speed  of  3.5  knots. 

Examination  of  the  maneuver  shows  that  a  limit  of  about 
0.25  radians  and  0.18  radians,  respectively,  was  set  by  the 
command  generation  logic  while  the  maximum  pitch  angle  of  40 
degrees  was  reached  and  maintained  after  16  seconds.  Also, 
while  the  vehicle  pitch  angle  is  returned  to  a  small  value, 
when  the  control  surfaces  are  returned  to  zero,  a  small 
residual  pitch  angle  is  left.   This  is  a  small  point  that 
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could  be  corrected  by  a  refinement  of  the  command  generation 
logic. 

What  is  of  interest  is  the  magnitude  of  the  final  depth 
error  generated  by  the  difference  between  linear  and  non- 
linear models.  While  the  command  generation  logic  drives 
the  linear  reference  model  to  the  targeted  depth  of  100  ft. , 
the  speed  reduction  in  the  AUV  only  provides  a  dive  to  87 
feet — clearly  indicating  the  need  for  corrective  control 
action. 

D.   EFFECT  OF  AUTOPILOT  CONTROL — BASELINE  CASE 

Figures  6.8  to  6.12  clearly  show  the  difference  the 
controller  makes  in  attaining  the  ordered  depth.  This  was 
the  first  simulation  run  using  the  full  19  state  controller 
for  control  in  the  heave/pitch  plane.  Although  excellent 
depth  control  was  achieved,  the  pitch  control  was  considered 
a  little  loose  resulting  mainly  from  the  mismatch  between 
model  and  AUV  speed.  Figure  6.12  shows  the  overshoot  of  the 
vehicle  pitch  during  the  maneuver.  The  overshoot  of  the 
pitch  also  is  the  primary  reason  for  the  vehicle  attaining 
ordered  depth  much  more  rapidly  than  the  model  as  shown  in 
Figure  6.9. 

Other  observations  include,  the  majority  of  the  control 
action  comes  from  the  stern  plane  which  worked  much  harder 
than  the  bow  planes.  Figures  6.8  and  6.11  show  the 
differences  in  control  actions  between  the  model  and  vehicle 
for  stern  and  bow  planes,  respectively. 
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Because  of  the  disparity  in  control  efforts  an  attempt 
to  equalize  the  relative  amount  of  control  actions  and  more 
closely  following  the  model  was  made.  As  discussed  in 
Chapter  IV,  the  control  weight  matrix  R  (Table  1)  was 
initially  set  up  to  penalize  the  rudder,  rpm  and  buoyancy 
inputs,  so  that  the  primary  control  actions  would  be  from 
the  bow  and  stern  planes  as  it  would  be  for  a  dive  maneuver. 
In  this  first  run  the  weights  were  equal  and  the  resulting 
control  gains  (Table  3)  for  the  stern  plane  were  much  higher 
than  for  the  bow  planes.  An  attempt  was  made  at  sharing  the 
control  actions  where  weights  of  the  R  matrix  were  adjusted 
to  penalize  the  stern  plane  and  put  more  control  effort  in 
the  bow  planes.  This  resulted  in  a  significant  loss  in  the 
stern  plane  gain  much  less  that  one  and  only  a  very  slight 
rise  in  the  bow  plane  gain.  Although  the  resulting 
simulation  showed  more  bow  plane  action  it  did  not  follow 
the  model  well  and  the  stern  plane  became  more  active  by  the 
feedback  action.  This  increased  activity  in  the  bow  and 
stern  planes  resulted  in  very  significant  speed  loss  and 
excessive  plane  use  was  considered  unacceptable. 

Upon  further  study  of  the  vehicle  and  its  dynamics,  the 
reason  for  the  inconsistency  in  control  actions  is  that  the 
model  maneuver  treats  bow  and  stern  planes  almost  equally  in 
their  effect  but  in  fact  the  force  and  moment  generated  by 
the  stern  plane  is  an  order  of  magnitude  more  significant 
than  that  of  the  bow  planes.   Therefore,  in  future  maneuver 
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TABLE  1 
TABLE  OF  £  WEIGHTS — BASELINE 


Pitch  Rate  Error 

Q(5,5)  0.6 

Q((5,14)  -0.6 

Q(14,5)  -0.6 

Q(14,14)  0.6 

Pitch  Angle  Errors 

0(11,11)  2.5 

0(11,16)  -2.5 

0(16,11)  -2.5 

0(16,16)  2.5 

Heave  Rate  Error 

0(3,3)  1.0 

0(3,13)  -1.0 

0(13,3)  -1.0 

0(13,13)  1.0 

Heave  Positional  Error 

0(9,9)  60.0 

Q(9,15)  -60.0 

Q(15,9)  -60.0 

Q(15,15)  60.0 
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TABLE  2 
TABLE  OF  R  WEIGHTS — BASELINE 

Rudder                 R(l,l)  1.0  x  104 

Starboard  Bow  Plane     R(2,2)  1.0  x  103 

Port  Bow  Plane          R(3,3)  1.0  x  103 

Stern  Plane            R(4,4)  1.0  x  103 

RPM                    R(5,5)  1.0  x  106 

Buoyancy               R(6,6)  l.OxlO6 


69 


TABLE    3 
CONTROL   GAINS 


O.OOOOOOOOOOE+OO 

0.4954081951E-08 

O.O00OO0O00OE+OO 

0.4644012840E-09 

0.2595669042E-05 

-0.4438610978E-04 

■0.5901765458E-07 

O.OOOOOOOOOOE+OO 

0.7605860458E-04 

O.OOOOOOOOOOE+00 

0.7216705136E-05 

0.4454871633E-01 

■0.6952859507E+00 

-0.9987243305E-02 

O.0O0000000OE+00 

0.7628508878E-04 

0.0000000000E+00 

O.7236512897E-05 

0.4459061566E-01 

•0.-6970854515E+00 

■0.9759326349E-02 

O.OO00000000E+OO 

■0.6689594082E-03 

0.0000000000E+00 

•0.5682397825E-04 

■0.6106134440E-01 

0.5192655967E+01 

•0.1004805424E+01 

0.0000000000E+00 

•0.4904055922E-10 

O.O000O00000E+00 

-0.4355770203E-11 

•0.1393062766E-07 

0.4070361389E-06 

•0.4242388239E-07 

O.00O0O00O00E+00 

0.8911601991E-10 

O.O0000OO000E+0O 

0.1128610385E-10 

0.1929779261E-06 

■0.1207295075E-05 

-0.4768694409E-06 


0.7258932550E-10 
0.3019079092E-04 
0.0000000000E+00 
0.4422721241E-04 
0.3025079273E-04 
0.1165944258E-05 

0.1114619576E-05 
0.4668891458E+00 
0.0000000000E+00 
0.6929793740E+00 
0.4676730097E+00 
0.1746945879E-01 

0.1117984364E-05 
0.4682061530E+00 
0.0000000000E+00 
0.6947686167E+00 
•0.4689957818E+00 
■0.1752998162E-01 

0.9857068418E-05 
■0.3859172641E+01 

O.OOOOOOOOOOE+OO 
•0.5158026393E+01 

0.3880347969E+01 

0.1856029149E+00 

■0.7210798044E-12 
0.2899780644E-06 
0.0000000000E+00 

■0.4049075188E-06 
0.2911221362E-06 
0.1269293021E-07 

0.1283218668E-11 
0.6522729441E-06 
0.0000000000E+00 
0.1210698470E-05 
•0.6471117447E-06 
0.6867469453E-08 


0.2575687868E-05 
0.2760571526E-08 
0.2752279831E-05 
0.0000000000E+00 
0.2764935447E-05 
0.1165974829E-05 

0.4422518793E-01 
0.4168961983E-04 
■0.4510255945E-01 
O.OOOOOOOOOOE+OO 
0.4530064631E-01 
•0.1747022491E-01 

■0.4426664311E-01 
0.4182848952E-04 

■0.4518982016E-01 
O.OOOOOOOOOOE+OO 
0.4538842427E-01 

■0.1753074334E-01 

0.6050596839E-01 
■0.4217829036E-03 

0.2348149885E+00 

O.OOOOOOOOOOE+00 
■0.2363129821E+00 

0.1855811163E+00 

0.1382443897E-07 
■0.2934508269E-10 

0.2184067568E-07 

O.OOOOOOOOOOE+OO 
■0.2195747233E-07 

0.1269214802E-07 

■0.1916238257E-06 
0.2538939491E-10 

•0.1217387526E-06 
O.OOOOOOOOOOE+OO 
0.1220747899E-06 
0.6880516105E-08 
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model  generation  it  should  be  noted  that  bow  plane  forces 
and  moments  are  more  subtle  and  should  be  used  for  fine 
depth  control  rather  than  for  major  depth  excursions. 
Considering  the  speed  mismatch,  the  overall  control  was 
considered  acceptable. 

E.   EFFECT  OF  TIGHTER  PITCH  CONTROL  WEIGHTING 

Due  to  the  unique  dynamics  of  the  vehicle  it  was  decided 
to  leave  the  control  weights  the  same  in  the  R  matrix,  as  it 
was  in  the  first  run,  with  the  understanding  that  the  model 
maneuver  algorithm  perhaps  wasn't  as  well  suited  for  this 
vehicle  as  it  could  have  been. 

With  the  R  matrix  fixed,  with  equal  weights  on  the  bow 
and  stern  planes,  it  was  decided  to  adjust  the  weights  in 
the  0.  matrix  to  try  to  gain  better  control  over  the  pitch, 
and  to  increase  the  pitch  error  gains.  The  weights  that 
were  adjusted  were  those  that  related  pitch  errors,  elements 
Q(ll,ll),  Q(ll,16),  Q(16,ll),  Q(16,16). 

When  these  elements  were  increased  by  a  factor  of  1000 
the  pitch  error  gains  increased  and  a  tighter  control  over 
pitch  was  achieved  as  shown  in  Figures  6.13  to  6.17. 

Comparing  Figures  6.7  and  6.17  shows  the  tighter  control 
gained  over  the  pitch.  With  the  tighter  control  gained  in 
pitch  a  slight  degradation  in  depth  control  was  observed. 
Figures  6.14  and  6.15  show  a  small  overshoot  in  ordered 
depth  for  the  vehicle  indicating  the  loosening  of  the  depth 
control  modes. 
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F.  FURTHER  PITCH  CONTROL  WEIGHTING 

The  0.  matrix  pitch  elements  were  further  increased  by  a 
factor  of  10  to  observe  the  correlation  between  depth  and 
pitch  control.  Again  Figures  6.18  to  6.22  show  a  sluggish 
response  in  depth  control  while  gaining  a  much  tighter 
control  over  pitch.  However,  in  this  case  the  command  for 
the  bow  planes  have  exceeded  their  physical  limits  and  are 
commanded  to  an  unreasonable  amount  as  shown  in  Figure  6.21. 

As  the  linear  controller  can  command  a  control  action 
greater  than  the  physical  limits  of  the  vehicle,  when  poor 
weights  are  selected,  logic  was  added  to  the  DSL  code  to 
limit  the  plane  action  to  plus  or  minus  .6  radians  on  the 
bow  and  stern  planes. 

Although  the  increased  weights  in  the  Q  matrix  gave  a 
better  pitch  response,  its  effects  on  tracking  control  were 
undesirable.  For  this  reason,  and  for  all  subsequent 
numerical  experiments,  it  was  decided  to  use  the  gains 
originally  calculated  in  the  baseline  run  and  the  original  0. 
matrix  weights. 

G.  EFFECT  OF  SPEED  MISMATCH  MODEL/VEHICLE 

The  major  issue  of  control  robustness  relates  to  the 
seriousness  of  speed  mismatch  between  model  and  AUV.  So  the 
next  task  was  to  test  this  controller  over  a  range  of 
vehicle  speeds,  3,  12  and  30  feet/sec. 

Using  the  controller  and  model  based  on  a  speed  of  6 
ft/sec,  the  simulation  was  run  using  a  vehicle  speed  of   12 
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ft/sec.  Figures  6.2  3  to  6.27  show  very  good  tracking 
ability  even  though  the  vehicle  was  going  twice  the  speed. 
Figure  6.24  shows  that  the  vehicle  went  twice  as  far  as  the 
model  to  reach  the  same  depth,  due  primarily  to  the  vehicle 
speed  being  double  that  of  the  model.  Figure  6.27  shows  the 
compensation  in  pitch  angle  to  achieve  desired  depth.  If 
the  controller  was  tighter  in  pitch  it  would  have  followed 
closer  in  this  figure  but  in  Figure  6.25  the  accurate 
trajectory  tracking  would  be  lost.  Again  this  goes  back  to 
the  type  of  control  needed  and  adjusting  of  the  weights  in 
the  Q   and  R  matrix  to  generate  satisfactory  control  gains. 

The  next  test  of  the  controller  was  an  attempt  to  run 
the  vehicle  at  a  speed  of  3  ft/sec  which  is  very  slow  and 
yet  try  to  use  a  model  speed  of  6  ft/sec.  The  primary 
motivation  was  to  see  if  one  set  of  gains  and  one  model 
could  be  used  for  all  maneuvers,  rather  than  recalculating 
gains  every  time  the  vehicle  changed  speeds;  a  test  of 
robustness  in  the  controller.  When  the  vehicle  was  operated 
at  3  ft/sec  the  vehicle  started  out  lagging  the  model  and 
then  control  errors  grew  while  the  controller  commanded  more 
and  more  action.  But,  since  the  vehicle  was  much  slower 
than  the  model  ordered,  depth  and  path  following  could  not 
be  achieved. 

To  alleviate  this  problem,  gains  were  recalculated  and 
the  model  was  run  at  3  ft/sec  (Figures  6.2  8  to  6.32)  when 
excellent  tracking  was  restored.   However,  at  the  slower 
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speed  (only  1.75  knots)  the  dive  maneuver  algorithm  used  was 
not.  sufficient  to  maintain  pitch  during  a  diving  trajectory. 
The  buoyant  moment  overcame  the  hydrodynamic  moment  from 
control  surfaces  and  ordered  depth  was  not  achieved.  This 
behavior,  however,  is  not  characteristic  of  the  controller 
but  rather  the  maneuver  logic,  and  as  far  as  the  controller 
is  concerned  it  was  able  to  follow  the  model  rather  nicely. 

Since  the  methodology  here  was  to  design  a  controller 
that  was  robust  enough  to  handle  a  wide  variety  of  reflexive 
type  maneuvers  over  a  range  of  speeds  it  is  most  likely  that 
the  vehicle  will  be  traveling  at  much  greater  speeds  when 
these  maneuvers  are  executed.  For  this  reason,  another 
simulation  run  was  made.  Again  the  control  weights  and 
gains  used  were  as  per  the  baseline  case  of  6  ft/sec.  The 
model  was  also  at  6  ft/sec  and  this  time  the  vehicle  was  at 
3  0  ft/sec.  Figures  6.33  to  6.37  show  once  again  excellent 
tracking  control,  and  like  the  12  ft/sec  case  tight  pitch 
control  was  eased  in  favor  of  accurate  depth  and  trajectory 
control,  which  is  desirable  not  to  have  the  vehicle 
violently  pitching  during  a  maneuver  which  may  result  in 
vehicle  equipment  damage. 

H.   EIGENVALUES — LINEAR  MODEL 

The  following  presents  a  table  of  the  eigenvalues  of  the 
baseline  model  at  6  ft/sec  forward  speed  together  with  the 
closed  loop  eigenvalues  found  using  the  baseline  weights. 
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TABLE  4 

OPEN  LOOP  EIGENVALUES 

Eiaenvalues 

Real  Part 

Imaginary  Part 

1 

-0.1600E-02 

0.0000E+00 

2 

-0.1500E-02 

0.0000E+00 

3 

-0.1700E-02 

0.0000E+00 

4 

-0.1800E-02 

0.0000E+00 

5 

-0.2100E-02 

0.0000E+00 

6 

-0.1000E-03 

0.0000E+00 

7 

-0.1663E+01 

0.0000E+00 

8 

-0.6579E+00 

0.0000E+00 

9 

-0.9553E+00 

0.0000E+00 

10 

-0.1122E+00 

0.7003E-02 

11 

-0.1122E+00 

-0.7003E-02 

12 

-0.3909E+00 

0.0000E+00 

13 

-0.1603E-01 

0.0000E+00 

14 

-0.9553E+00 

0.0000E+00 

15 

-0.3908E+00 

0.0000E+00 

16 

-0.1423E-01 

0.0000E+00 

17 

-0.3000E-03 

0.0000E+00 

18 

-0.4000E-03 

0.0000E+00 

19 

-0.5000E-03 

0.0000E+00 
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TABLE  5 

CLOSED  LOOP  EIGENVALUES 

Eigenvalues 

Real  Part 

Imacrinarv  Part 

1 

-0.1600E-02 

0.0000E+00 

2 

-0.1500E-02 

0.0000E+00 

3 

-0.1700E-02 

0.0000E+00 

4 

-0.2100E-02 

0.0000E+00 

5 

-0.1663E+01 

0.0000E+00 . 

6 

-0.9888E+00 

0.0000E+00 

7 

-0.3456E+00 

0.4402E+00 

8 

-0.3454E+00 

-0.4402E+00 

9 

-0.6579E+00 

0.0000E+00 

10 

-0.4868E+00 

0.0000E+00 

11 

-0.9553E+00 

0.0000E+00 

12 

-0.1122E+00 

0.7003E-02 

13 

-0.1122E+00 

-0.7003E-02 

14 

-0.3908E+00 

0.0000E+00 

15 

-0.1423E-01 

0.0000E+00 

16 

-0.1000E-03 

0.0000E+00 

17 

-0.3000E-03 

0.0000E+00 

18 

-0.4000E-03 

0.0000E+00 

19 

-0.5000E-03 

0.0000E+00 
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VII.   SUMMARY.  CONCLUSIONS.  AND  RECOMMENDATIONS 

A.  SUMMARY 

This  thesis  presents  a  study  of  Model  Reference  Controls 
for  an  Autonomous  Underwater  Vehicle.  The  approach  to  the 
design  and  testing  of  a  model  following  autopilot  included: 

1.  Selection  of  a  suitable  submersible  was  selected,  one 
that  displayed  many  attributes  for  potential  AUV 
missions.  One  in  which  all  the  hydrodynamic 
characteristics  were  well  studied  and  data  were 
obtainable. 

2.  The  tailoring  of  the  existing  equations  of  motion  to 
gain  control  over  all  six  degrees  of  freedom. 

3.  The  development  of  a  linearized  model  and  programming 
the  linearized  and  non-linear  models  for  simulation 
using  Dynamic  Simulation  Language  (DSL) . 

4 .  The  development  of  a  19  state  controller  for  dive 
plane  maneuvers.  Maneuvers  that  could  be  termed 
reflexive. 

5.  The  development  of  logic  for  a  command  generation 
system  for  a  dive  maneuver. 

6.  Observation  of  the  effects  on  the  control  gains  by 
varying  the  weights  in  the  minimizing  function  J. 

7.  The  testing  of  the  command  generation  logic  and  the 
controller  over  a  wide  range  of  speeds  using  only  one 
set  of  calculated  gains  based  on  one  speed  of  6 
ft/sec. 

B.  CONCLUSIONS 

In  this  study,  a  methodology  was  developed  to  the  design 
of  a  model  following  autopilot  that  could  be  used  in  an 
Autonomous  Underwater  Vehicle.  A  19  state  controller  was 
designed  for  automatic  control  of  maneuvers  in  the  dive 
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plane.  This  controller  displayed  excellent  trajectory 
following  characteristics  and  exhibited  a  high  degree  of 
robustness  over  a  five  to  one  speed  range. 

The  model  following  autopilot  was  designed  to  follow 
trajectories  generated  from  a  preprogrammed  maneuver 
algorithm.  This  maneuver  logic  proved  to  be  workable  and 
could  easily  be  developed  for  a  wide  variety  of  maneuvers  to 
be  stored  on-board  in  a  computer. 

In  this  study  maneuver  logic  was  created  for  one  such 
maneuver,  a  dive  maneuver,  and  was  followed  by  the  designed 
autopilot.  The  algorithm  used  for  the  dive  maneuver  was 
crude  but  sufficiently  proved  that  the  design  methods  are 
sound. 

Some  difficulties  in  perfect  trajectory  following  occur 
because  of  speed  mismatch  between  model  and  vehicle,  and  an 
improvement  in  modelling  speed  loss  during  maneuvers  would 
be  worthwhile. 

C.   RECOMMENDATIONS 

Because  the  concept  of  Autonomous  Underwater  Vehicles  is 

fresh   and   significant   progress   has   been  made   in   the 

computational   abilities   of   modern   computers,   a   wide 

diversification   of   technological   avenues   need   to   be 

explored.      Specific   to   this   study   the   following 

recommendations  are  presented. 

1.   An  implementation  of  the  full  3  0  state  dynamically 
coupled  controller  in  an  AUV  should  be  the  ultimate 
goal  of  this  project.   In  particular,  the  influence 
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for  forward  speed  changes  should  be  reflected  in  the 
maneuver  command  generation  model  for  greater  control 
accuracy. 

2 .  Parallel  efforts  should  be  carried  forward  with  the 
development  of  many  maneuver  algorithms  that  could  be 
stored  in  the  AUV's  "bag"  of  maneuvers. 

3.  Although  this  controller  was  designed  specifically  for 
the  control  of  an  AUV  with  an  unusual  geometry,  it  can 
and  should  be  tested  on  underwater  vehicles  with  other 
geometries. 
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APPENDIX  A 

SIX-DEGREE-OF-FREEDOM  EQUATIONS  OF  MOTION 
AND  EULER  ANGLE  RATES 


SURGE  EQUATION  OF  MOTION 

m[u  -  vr  +  wq  -  xG(q2  +  rr)  +  yG(pq  -  r)  +  zG(pr  +  q) ] 

=  §  *4[Xpp  p2  +  Xq'q  q2  +  Xr'r  r2  +  Xpr  pr] 

■     *■  i  * 

+    |  £3[X^  u  +  Xwq  wq  +   Xvp  vp  +  Xvr  vr 

+   u<3(xq6s    6s   +   xq5b/2    6bp   +   xq6b/2     ^ 

+  Xr6r  ur6r]  (A-l) 

+    §  *2[*vv  v2   +  xww  w2   +  XvSr  uv6r 

+  uw(XW(Ss    6S  +   xw6b/2   <$bs 

1  6 

+  xw6b/2      bp> 

~        ■  _2  '  x2  '  *2 

+   uZ(x6s6s  6s   +   x6b6b/2    6bp   +   x<5b<5b/2    °bs 

+  x6r6r    6r)  ] 
-    (W-B)    sin    G  +    |  9?    Xq6sn  uq6s     e(n) 

+    f  ^2Cxw6sn  UW(5s  +   x6s5sn  u2   6s]    £  (n) 
2  Aprop 
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SWAY  EQUATION  OF  MOTION 

m[v  +  ur  -  wp  +  xG(pq  +  r)  -  yG(p2  +  r2) 

+  ZG(qr  -  p)] 


i 


=   f  ^4[Yp  p  +  Y£  r  +  Ypq  pq  +  Yqr  qr] 

+  |  l3[Vv   v  +  Yp  UP  +  Y^  ur  +  Yvg  vq 

+  Ywpwp+  Ywr  wr]  (A-2) 

+  §  £2[YV  uv  +  Y^  vw  +  Y$r   u2  6r] 

x 

nose 

-  £-/      £cDy  h(x)  (v+xr)2 


x ,  . , 
tail 


+  CDz  b(x)  (w-xq)2]  (v+**-)  dx 

cf  {x) 


+    (W-B)  cos  6  sin  (J> 
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HEAVE  EQUATION  OF  MOTION 

m[w  +  uq  +  vp  +  xG(pr  -  q)  +  yG(qr  +  p) 

-    zG(p2    +   q2)] 

i 

=       |   £4[Zg   q   +    Zpp   p2    +    Zpr   pr   +    Zrr   r2  ] 

i 

+    J   JlJ[Z^  w  +    Z^  uq  +    Zvp  vp   +    Zvr  vr] 

+    |  I2    [Z„  uw  +    Zw  v2  (A-3) 

+   u2(Z6s    6S   +    Z6b/2    6bs   +    z6b/2    5bp] 

x 
nose 

"    f  /  CcDy  h(x)  (v+xr)2 


xtail 


+   CDz    b(x)(w-xq)2]     i,W   -g|    dx 

cf  l    ; 

+    (W-B)    cos  6  cos  <j> 

+    §  £3    zqn  u<2     ^n) 

+     2^tzwn  uw  +   z5sn  u2   <5S3    e  (n) 
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ROLL  EQUATION  OF  MOTION 

Jx  P  +    (Jz   -   xy)    <3r  +   Ixy(Pr  "  <3)    ""   ^zCS2   "  r2) 

•  •  • 

-   IXz(P(3+r)    +  mCyG(w  -  uq  +  vP)    -   zG(v  +  ur  -  wp)  ] 

f  £5[Kp  p  +  K'r   r  +  Kpq  pq  +  Kgr  qr] 

P      ' 
+  2"-^4[K^  v  +  Kp  up  +  Kr  ur  +  Kvq  vq  (A-4) 

i         i 

+  Kwp  wp  +  Kwr  wr] 

+  2  ^3[4  uv  +  K^  vw  +  u2(K^b/2  «bp  +  K6'b/2  «bs)  ] 
+  (y^W  -  Ybb)  cos  e  cos  $  ~  (zgw  ~  ZBB)  cos  e  sin ^ 

+  f  *4  KE>n  UP  e  (n) 

+  £  }3  n2  if 

2       ^prop 
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PITCH  EQUATION  OF  MOTION 

•  •  • 

Jy  3  +    (Jx  "   Jz)    Pr  ~   Jxyter  +  P)    +  Iyz(pq  -  r) 

*  • 

+    Ixz(P2"r2)    "  m[xG(w   "   uq  +  vp)    -    zG(u   -   vr  +  wq] 

'         •  I  I  f 

=    |     *5[Mq   q  +  Mpp  p2    +  Mpr  pr  +  Mrr   r2 ] 


i        i         i 

xw  w  -r  nq  uq  +  Mvp  vp  +  Mvr 


+  §■  £4[M™  w  +  Ma  uq  +  M^  vp  +  Mvr  vr] 


+    |   £3[M^  uw  +  M^  v2 


•  • 


+   u^(M6s    6S   +   M6b/2    6bp   +   M6b/2    6bs) ] 

x 

nose 

+    §  /  CcDy  h<x)     (v+xr)2 

xtail 

+   CDz   b(x)     (w-xq)2]    ^~X{1]    x  dx 

-    (xGW   -   xBB)    cos    0  cos    <j>    -    (zGW   -    zBB)    sin    G 


+    §  A4   Mqn  u<3    e(n) 


+    f  £3CM*!n  uw  +  M6sn  u2    6sl     e(n) 
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YAW   EQUATION   OF   MOTION 

Iz    r   +    (Iy   -    Ix)pq   -    IXy(P2-q2)    "    Iyz(Pr+(3) 

+   Ixz(qr-p)    +  m[xG(v  +  ur  -  wp)    -  yG(u  -  vr  +  wq) ] 

=    f  £5[Np  p  +  Nr  r  +  Npq  pq  +  Nqr  qr] 
+    j  &4[N^  v  +  Np  up  +  Njl  ur  +  Nvq  vq 

+  N^  wp  +  Nwr  wr] 

+    2    £3CNv  uv  +  Nvw  w  +  N6r  u2    6r] 

-    f  /Xn°Se   [CDy  h(x)     (v+xr)2 
xtail 

+  cDz  b(x)  (w-xq)2]  uv+^j;  xd* 

+  (xGW  -  xBB)  cos  9  sin 4)  +  (y^W  -  ysB)  sin  e 

+  §  p  u2  Nj;rop 
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Euler  Angle  Rates 
<J>     =     p  +  q  sin    <f>  tan    9  +   r  cos    <$>  tan 

9  =  q  cos  cf)  -  r  sin  <J) 

i)j      sin  ^ 

V  -  q  n-  +  r 


sin  y     cos  <$> 
cos  S"     cos  9" 


Inertial  Position  Rates 

x0  =  uc0  +  u  cos  $   cos  ® 

+  v[cos    ip  sin    9  sin    <J>  -   sin    \p  cos    <$>] 

+  w[cos    ip  sin    9  cos    <j>  +   sin    ip  sin    ({>] 

• 

y       =  vcq   +  u   sin    \p  cos    9 

+  v[sin    ty  sin    9  sin    $  +   cos    ip  cos    <f>] 
+  w[sin    \p  sin    9  cos    <j>  -  cos    ip  sin    <J>] 

z0     =  wc0   "  u  sin    9 

+  v  cos    9  sin    \p 

+  w   cos    9  cos    cj) 

Crossflow  Velocity 

u    .(x)=    [(v  +  xr)2   +    (w  -  xq)2]V2 
cf 
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APPENDIX  B 
DSL  LISTING  FOR  AUV  SIMULATION 

TITLE  AUTONOMOUS  UNDERWATER  VEHICLE  (AUV)  SIMULATION 

D      C0MM0N/BL0CK1/  MMINV(6,6),  MM(6,6),  AA(12,12),  BB(6,6) 

D      COMMON/BLOCK2/  B(6 , 6) , A(12, 12) ,  UMOD(6) ,GKK(6 ,21) 


,YQR  = 
,YVQ  = 


D      COMMON/BLOCK3/  F(l2),  FP(6),  UCF(4) 

D      C0MM0N/BL0CK4/  G4(4) ,GK4(4) ,BR(4) ,HH(4) 

D      COMMON/ BLOCK5/  XDOT(12) ,XDOTX(12) ,  XDOTU(6) 

FIXED  N^A^DGT^ER^AST^K^J^KK,! 

INTEGER 

ARRAY  WKAREA(54),  X(12) 

CONST 
* 

*  LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 
* 

CONST  XPP  =  ,XQQ  =  ,XRR  =  ,XPR  = 

XUDOT=  ,XWQ  =  ,XVP  =  ,XVR  = 

XQDS=  ,XQDB=  ,XRDR=  ,XW  = 

XWW  =  ,XVDR=  ,XWDS=  ,XWDB= 

XDSDS=  ,XDBDB=  ,XDRDR=  ,XQDSN= 

XWDSN=  ,XDSDSN= 

*  LATERAL  HYDRODYNAMIC  COEFFICIENTS 

CONST  YPDOT=  ,YRDOT=  ,YPQ  = 

YVDOT=  ,YP  =  ,YR  = 

YWP  ■  ,YWR  =  ,YV  =  ,YVW  = 

YDR  =  ,CDY  = 

*  NORMAL  HYDRODYNAMIC  COEFFICIENTS 

CONST  ZQDOT=  ,ZPP  =  ,ZPR  =  ,ZRR  = 

ZWDOT=  ,ZQ  =  ,ZVP  =  ,ZVR  = 

ZW  =  ,ZVV  =  ,ZDS  =  ,ZDB  = 

ZQN  =  ,ZWN  =  ,ZDSN=  ,  CDZ  = 

*  ROLL  HYDRODYNAMIC  COEFFITIENTS 
* 

CONST  KPDOT=         ,  KRDOT=         ,KPQ  =  ,KQR  = 

KVDOT=  ,  KP  =  ,KR  =  ,KVQ= 

KWP  =  ,  KWR  =  ,KV  =  ,KVW  = 

KPN  =         ,  KDB  = 
* 

*  PITCH  HYDRODYNAMIC  COEFFICIENTS 

CONST  MQDOT=  ,  MPP  =  ,MPR  =  ,MRR  = 

MWDOT=  ,  MQ  =  ,MVP  =  ,MVR  = 

MW  =  ,  MVV  =  ,MDS  =  ,MDB  = 

MQN  =  ,  MWN  =  ,MDSN  = 

*  YAW  HYDRODYNAMIC  COEFFICIENTS 

CONST  NPDOT=  ,  NRDOT=  ,NPQ  =  ,NQR  = 

NVDOT=  ,  NP  =  ,NR  =  ,NVQ  = 

NWP  =  ,  NWR  =  ,NV  =  ,NVW  = 
NDR  = 

*  MASS  CHARACTERISTICS  OF  THE  FLOODED  auv 

CONST  WEIGHT  =        ,  BOY  =  ,VOL  =  ,XG  = 

YG  =  ,  ZG  =  ,XB  =  ,ZB  = 
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IX  =  ,  IY  =  ,IZ  =  ,IXZ  = 

IYZ  =  ,  IXY  =  ,YB  = 

L  =  ,  RHO  =  ,G  =  ,NU  = 

AO  =  ,KPROP  =         ,NPROP  =     ,     X1TEST= 

* 

*  INPUT  INITIAL  CONDITIONS  HERE  IF  REQUIRED 
INITIAL 

*  INITIALIZE  ALL  MATRICES  AND  ARRAYS  TO  ZERO 

N  =  6 

DO  2   J  =  1,N 

JJ=  J+N 

DO   1  K  =  1,N 

KK=  K+N 
KKK=  KK  +  N 
MMINV(J,K)  =  0.0 
X(J)  =  0.0 
X(JJ)  =  0.0 
XDOT(J)  =  0.0 
XDOT(JJ)  =  0.0 
XDOTX(J)  =  0.0 
XDOTX(JJ)  =  0.0 
XDOTU(J)  =  0.0 
UMOD(J)  =0.0 
MM(J,K)  =  0.0 
BB(J,K)  =  0.0 
B(J,K)  =  0.0 
AA(J,K)=  0.0 
AA(JJ,KK)=  0.0 
AA(J,KK)=  0.0 
AA(JJ,K)=  0.0 
A(J,KK)=  0.0 
A(JJ,K)  =  0.0 
A(J,K)  =  0.0 
A(JJ,KK)=  0.0 
GKK(J,K)=  0.0 
GKK(J,KK)=0.0 
GKK(J,KKK)=0.0 

1  CONTINUE 

2  CONTINUE 

*  INPUT  THE  LINEARIZATION  POINT  PARAMETERS 
* 

UO  =6.0 
VO  =  0.0 
WO  =  0.0 
PO  =  0.0 
QO  =  0.0 
RO  =  0.0 
PHIO  =  0.0 
THETAO  =0.0 
PSIO  =  0.0 
SUM  =  0.0 
JFLAG  =  0 
I FLAG  =  0 
KFLAG  =  0 
ZORD  =  100.0 

*  INPUT  THE  MODEL  STATES  INITIAL  CONDITIONS 
* 

UM  =  6.0 
VM  =  0.0 
WM  =  0.0 
PM  =  0.0 
QM  =  0.0 
RM  =  0.0 
XPOSM  =0.0 
YPOSM  =0.0 
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ZPOSM  =0.0 
PHIM  =0.0 
THETAM  =0.0 
PSIM  =  0.0 

*  INPUT  THE  VEHICLE  INITIAL  CONDITIONS 

U  =  6.0 
V  =  0.0 
W  =  0.0 
P  =  0.0 
Q  =  0.0 
R  =  0.0 
XPOS  =0.0 
YPOS  =0.0 
ZPOS  =0.0 
PSI  =  0.0 
THETA  =0.0 
PHI  =  0.0 

*  INITIALIZE  THE  CONTROLS 

DELB0Y=  0.0 
DBS=  0.0 
DBP=  0.0 
DS  =  0.0 
DR  =  0.0 
RPM  =  250.00 
LATYAW  =0.0 
NORPIT  =0.0 

DEFINE  LENGTH  FRACTIONS  FOR  GAUSS  QUADUTURE  TERMS 

G4(l)  =  0.069431844 

G4(2)  =  0.330009478 

G4(3)  =  0.669990521 

G4(4)  =  0.930568155 

DEFINE  WEIGHT  FRACTIONS  FOR  GAUSS  QUADUTURE  TERMS 

GK4(1)  =  0.1739274225687 

GK4  2)  =  0.3260725774312 

GK4(3)  =  0.3260725774312 

GK4  4)  =  0.1739274225687 

*  DEFINE  THE  BREADTH  BB  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 
* 

BR(1)  =  75.7/12 

BR(2)  =  75.7/12 

BR(3)  =  75.7/12 

BR(4)  =  55.08/12 

HH(1)  =  16.38/12 

HH(2)  =  31.85/12 

HH(3)  =  31.85/12 

HH  4)  =  23.76/12 

MASS  =  WEIGHT/G 

DIVAMP  =  DEGSTN^O. 0174532925 
RUDAMP  =  DEGRUD^O. 0174532925 


* 

*  THE  LINEAR  PROPULSION  MODEL 

*  ETA  =  0. 012*500. 0/UO 
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ETA  =1.0 

RE  =  U0*L/NU 

CDO  =   .00385  +  (1.296E-17)*(RE  -  1.2E7)**2 

CT  =  0.008*L**2*ETA*ABS(ETA)/(A0) 

CT1  =  0.008*L**2/(A0) 

EPS  =  -1.0+(SQRT(CT+1.0)-1.0)/(SQRT(CT1+1.0)-1.0) 

XPROP  =  CDO*(ETA*ABS(ETA)  -  1.0) 

MASS  =  WEIGHT/G 

DIVAMP  =  DEGSTN*0. 0174532925 

RUDAMP  =  DEGRUD^O. 0174532925 

*     CALCULATE  THE  MASS  MATRIX 

MM(1,1)  =  MASS  -((RHO/2)*(L**3)*XUDOT) 
MM(1,5)  =  MASS*ZG 
MM (1,6)  =  -MASS*YG 


MM(2,2)  =  MASS  - ( (RHO/2)*(L**3)*YVDOT) 
MM(2,4)  =  -MASS*ZG  - ( (RH0/2)*(L**4)*YPD0T) 
MM(2,6)  =  MASS*XG  -  ((RH0/2)*(L**4)*YRD0T) 


MM(3,3)  =  MASS  -  ( (RHO/2)*(L**3)*ZWDOT) 

MM (3, 4)  =  MASS*YG 

MM(3,5)  =  -MASS*XG  - ( (RHO/2)*(L**4)*ZQDOT) 

MM(4,2)  =  -MASS*ZG  -  ( (RH0/2)*(L**4)*KVD0T) 

MM (4 ,3)  =  MASS*YG 

MM(4,4)  =  IX  -  ((RHO/2)*(L**5)*KPDOT) 

MM(4,5)  =  -IXY 

MM(4,6)  =  -IXZ  -((RHO/2)*(L**5)*KRDOT) 

MM(5,1)  =  MASS*ZG 

MM(5,3)  =  -MASS*XG  - ( (RH0/2)*(L**4)*MWD0T) 

MM(5,4)  =  -IXY 

MM(5,5)  =  IY  -((RHO/2)*(L**5)*MQDOT) 

MM(5,6)  =  -IYZ 

=  -MASS*YG 

=  MASS*XG  -((RH0/2)*(L**4)*NVD0T) 

=  -IXZ  -  ((RHO/2)*(L**5)*NPDOT) 

=  -IYZ 

=  IZ  -  ((RHO/2)*(L**5)*NRDOT) 


MM( 

6'X 

MM< 

6'2 

MM( 

6,4 

MM< 

6,5 

MM< 

6,6 

LAST  =  N*N+3*N 
DO  20  M  =  1,LAST 
WKAREA(M)  =0.0 
20     CONTINUE 


IER  =  0 

IA  =  6 

IDGT  =  4 

WRITE(  8,400)((MM(I,J),  J  =  1,6),I  =  1,6) 

CALL  LINV2F(MM,N,IA,MMINV, IDGT, WKAREA, IER) 

*  WRITE(  8,400)((MMINV(I,J),  J  =  1,6), I  =  1,6) 
*00    FORMAT (6E 12. 4) 

*  CALCULATE  THE  A  MATRIX  FOR  THE  LINEAR  MODEL 

* 
* 


A(l,l)  =  RHO/2*L**3*(XQDS*DS*Q0+XQDB/2*DBP*Q0+XRDR*R0*DR)+. . . 
RHO/2*L**2*(XVDR*V0*DR+XWDS*DS*W0+XWDB/2*DBP*W0  +  .. 
2*U0*(XDSDS^DS**2  +  XDBDB/2*DBP**2  +  XDRDR*DR**2) )+ 
RHO/2*L**3*XQDSN*Q0*DS*EPS+RHO/2*L**2*(XWDSN*W0*DS  +  . 
2*XDSDSN*U0*DS**2)*EPS+RHO*L**2*U0*XPROP+RHO/2*L**3* 
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A(3 


1(3 

A(3 

A(4 


A(4 
A(4 
A(4 
A(4 
A(4 
A(4 
A(4 

A(5 


A(5 

A(5 
A(5 


XQDB/2*DBS*Q0+RHO/2*L**2*XWDB/2*DBS*W0+RHO*L**2*U0*. . . 
XDBDB/2*DBS**2 

2)  =  MASS*R0+RHO/2*L**3*(XVP*P0+  XVR*RO)  +  RHO/2*L**2*  ... 

(2*XVV*V0  +  XVDR*UO*DR) 

3)  =  -MASS*QO  +  RHO/2*L**3*(XWO*Q0)+RHO/2*L**2*(2*XWW*W0+... 

XWDS*DS*UO+ (XWDB/2*DBP+XWDB/ 2*DBS ) *UO  +XWDSN*UO*DS*EPS ) 

4)  =    -MASS*YG*QO-MASS*ZG*RO+   RHO/2*L**4*(2*XPP*P0+XPR*R0) . . . 

+   RHO/2*L**3*(XVP*V0) 

5)  =   -MASS*W0+2*MASS*XG*Q0    -MASS*YG*P0+RHO/2*L**4*2*XQQ*Q0 . . 

+RHO/2*L**3MXWQ*W0+XQDS*DS*U0+XQDB/2*DBP*U0)+RHO/2*    .  . 
L**3*XQDSN*U0*DS*EPS+RHO/2*L**3*XQDB/2*DBS*U0 

6)  =  MASS*V0+2*MASS*XG*R0-MASS*ZG*P0+RHO/2*L**4*(2*XRR*R0.. . 

+   XPR*PO)    +   RHO/2*L**3*(XVR*V0   +   XRDR*UO*DR) 
11)=   -(WEIGHT   -    BOY)*COS(THETAO) 

1)  =   -MASS*R0+RHO/2n,**3MYP*P0+YR*R0)+RHO/2*L**2*(YV*V0+... 

2*YDR*U0*DR) 

2)  =  RHO/2*L**3*YVQ*Q0+RHO/2*L**2*(YV*U0+YVW*W0) 

3)  =   MASS*PO+   RHO/2*L**3*(YWP*P0+YWR*R0)+RHO/2*L**2*YVW*V0 

4)  =   MASS*W0-MASS*XG*Q0+2*MASS*YG*P0+RHO/2*L**4*YPQ*Q0+... 

RHO/2*L**3*(YP*U0+   YWP*WO) 

5)  =   -MASS*XG*P0-MASS*ZG*R0+RHO/2*L**4*(YPQ*P0+YQR*R0)    +... 

RHO/2*L**3*YVQ*V0 

6)  =   -MASS*U0+2*MASS*YG*R0-MASS*ZG*Q0+RHO/2*L**4*YQR*Q0   +... 

RHO/2*L**3*(YR*U0   +  YWR*WO) 
10)=    (WEIGHT   -    BOY)*COS(THETA0)*COS(PHI0) 
11)=   -(WEIGHT   -   BOY)*SIN(THETA0)*SIN(PHI0) 

1)  =  MASS*Q0+RHO/2*L**3*ZQ*Q0+RHO/2*L**2MZW*W0+2*U0*ZDS*DS. 

+2*U0*ZDB/2*DBP+(ZWN*W0+2*ZDSN*U0*DS)*EPS)+RHO/2*L**3*. 
ZQN*QO*EPS+  RHO/2*L**2*2*U0*ZDB/2*DBS 

2)  =   -MSS*P0+RHO/2*L**3*(ZVP*P0+ZVR*R0)+RHO/2*L**2*2*ZVV*V0 

3)  =  RHO/2*L**2*(ZW*U0   +  ZWN*UO*EPS) 

4)  =   -MASS*V0-MASS*XG*R0+2*MASS*ZG*P0+  RH0/2*L**4*(2*ZPP*. . . 

PO   +  ZPR*RO)    +   RHO/2*L**3*ZVP*V0 

5)  =  MASS*UO   -   MASS*YG*R0+2*MASS*ZG*Q0+RHO/2*L**3*ZQ*U0   +... 

RHO/2*L**3*ZQN*U0*EPS 

6)  =-MASS*XG*P0-MASS*YG*Q0+RHO/2*L**4*(ZPR*P0+2*ZRR*R0)+.  .  . 

RHO/2*L**3*ZVR*V0 
10)=   -(WEIGHT    -    BOY)*COS(THETA0)*SIN(PHI0) 
11)=   -(WEIGHT    -    BOY)*SIN(THETA0)*COS(PHI0) 

1)  =  MASS*YG*QO    +   MASS*ZG*RO    +   RHO/2*L**4*(KP*P0    +    ... 

KR*R0)+RHO/2*L**3*(KV*V0+2*U0*(KDB/2*DBP-KDB/2*DBS) )+. . 
RHO/2*L**3*U0*KPROP+  RHO/2*L**4*KPN*P0*EPS 

2)  =  -MASS*YG*PO  +  RHO/2*L**4*KVQ*Q0  +  RHO/2*L**2*(KV*U0 . . . 

+  KVW*WO) 

3)  =  -MASS*ZG*PO  +  RHO/2*L**4*(KWP*P0  +  KWR*RO)  +  ... 

RHO/2*L**3*KVW*V0 

4)  =  -IXY*RO  +  IXZ*QO  -  MASS*YG*VO  -  MASS*ZG*WO  +  ... 

RHO/2*L**5*KPQ*Q0  +  RHO/2*L**4*(KP*U0+KWP*W0) 

5)  =  -IZ*RO  +  IY*RO  +  2*IYZ*Q0  +  IXZ^PO  +  MASS*YG*UO  +... 

RHO/2*L**5*(KPQ*P0  +  KQR*RO)  +  RH0/2*L**4*KVQ*V0 

6)  =  -IZ*QO  +  IY*QO  -  2*IYZ*R0  +  MASS*ZG*UO  +  ... 

RHO/2*L**5*KQR*QO  +  RHO/2*L**4*(KR*U0+KWR*W0) 
10)=  -(YG*WEIGHT-YB*BOY)*COS(THETA0)*SIN(PHI0> 

-(ZG*WEIGHT-ZB*BOY)*COS(THETA0)*COS(PHI0 
11)=  -(YG*WEIGHT-YB*BOY)*SIN(THETA0)*COS(PHI0' 

+(ZG*WEIGHT-ZB*BOY)*SIN(THETA0)*SIN(PHI0' 


1)  = 


-MASS*XG*QO  +  RHO/2*L**4*MQ*Q0_+  RHO/2*L**3*MW*W0 , +. . . 


RHO/2*L**3*U0*(MDS*DS+MDB/2*DBP)  +  RHO/2*L**4*MQN*Q0' 
EPS  +  RHO/2*L**3*(MWN*W0  +  2*MDSN*U0*DS)*EPS+. . . 
RHO/2*L**3*U0*MDB/2*DBS 

MASS*XG*PO  +  MASS*ZG*RO  +  RHO/2*L**4*(HVP*P0  +  ... 
MVR*RO)  +  RHO*L**3*MVV*V0 

-MASS*ZG*QO  +  RHO/2*L**3*MW*U0  +  RHO/2*L**3*MWN*U0*EPS 
-IX*RO  +  IZ*RO  -  IYZ*QO  -  2*IXZ*P0  +  MASS*XG*VO  +  ... 
RHO/2*L**5*(2*MPP*P0  +  MPR*RO)  +  RHO/2*L**4*MVP*V0 
A(5,5)  =  IXY*RO  -IYZ^PO  -  MASS*XG*UO  -MASS*ZG*WO  +  RHO/2*... 


2)  = 
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L**4*MQ*U0  +  RHO/2*L**4*MQN*U0*EPS 
A(5,6)  =  -IX*PO  +  IZ*PO  +  IXY*QO  +  2*IXZ*R0  +  MASS*ZG*VO  +... 

RHO/2*L**5MHPR*P0+2*MRR*R0)+RHO/2*L**4*MVR*V0 
A(5,10)=  (XG*WEIGHT-XB*BOY)*COS (THETAO )*SIN(PHIO) 
A(5,ll)=  (XG*WEIGHT-XB*BOY)*SIN(THETA0)*COS(PHI0)  -  ... 

(ZG*WEIGHT-ZB*BOY)*COS (THETAO) 

A(6,l)  =  -MASS*XG*R0  +  RHO/2*L**4*(NP*P0  +NR*RO)  +  RHO/2*... 

L**3*(W*V0+2*NDR*U0*DR)+RHO*L**3*U0*NPROP 
A(6,2)  =  -MASS*YG*RO  +  RHO/2*L**4*NVQ*Q0  +  RHO/2*L**3*(NV*U0+. . 

NVW*WO) 
A(6,3)  =  MASS*XG*P0  +  MASS*YG*QO  +  RHO/2*L**4*(NWP*P0+NWR*R0)+ ■ 

RHO/2*L**3*NVW*V0 
A(6,4)  =  -IY*QO  +  IX*QO  +  2*IXY*P0  +IYZ*RO  +  MASS*XG*WO+. . . 

RHO/2*L**5*NPQ_*QO  +  RH0/2*L**4*(NP*U0+NWP*W0) 
A(6,5)  =  -IY*PO  +  IX*PO  -  2*IXY*Q0  -  IXZ*RO  +  MASS*YG*WO+. . . 

RHO/2*L**5*(NPQ*PO+NQR*RO)  +  RHO/2*L**4*NVQ*V0 
A(6,6)  =  IYZ*PO  -IXZ*QO  -  MASS*XG*UO  -MASS*YG*V0  +  ... 

RHO/2*L**5*NQR*QO  +  RHO/2*L**4*(NR*U0  +NWR*WO) 
A(6,10)=  (XG*WEIGHT-XB*BOY)*COS (THETAO )*COS (PHIO) 
A(6,ll)=  -(XG*WEIGHT-XB*BOY)*SIN(THETAO)*SIN(PHIO)  +... 

(YG*WEIGHT-YB*BOY)*COS (THETAO) 

A(7,l)  =  COS(PSIO)*COS(THETAO) 

A(7,2)  =  COS(PSIO)*SIN(THETAO)*SIN(PHIO)  -  SIN(PSIO)*COS (PHIO) 
A(7,3)  =  COS(PSIO)*SIN(THETAO)*COS(PHIO)  +  SIN(PSIO)*SIN(PHIO) 
A(7,10)=  VO*COS(PSIO)*SIN(THETAO)*COS(PHIO)  +  VO*SIN(PSIO)* .  .  . 

SIN(PHIO)  -  WO*COS(PSIO)*SIN(THETAO)*SIN(PHIO)  +  ... 

WO*SIN(PSIO)*COS(PHIO) 
A(7,ll)=  -UO*COS(PSIO)*SIN(THETAO)  +  VO*COS (PSIO)*COS (THETAO)* , 

SIN(PHIO)  +  WO*COS(PSIO)*COS(THETAO)*COS(PHIO) 
A(7,12)=  -UO*SIN(PSIO)*COS(THETAO)  -  VO*SIN(PSIO)*SIN(THETAO)* , 

SIN(PHIO)  -  VO*COS(PSIO)*COS(PHIO)  -  WO*SIN(PSIO)*. . . 

SIN(THETAO)*SIN(PHIO)  +  WO*COS (PSIO)*SIN(PHIO) 

A(8,l)  =  SIN(PSIO)*COS(THETAO) 

A(8,2)  =  SIN(PSIO)*SIN(THETAO)*SIN(PHIO)  +  COS (PSIO)*COS (PHIO) 
A(8,3)  =  SIN(PSIO)*SIN(THETAO)*COS(PHIO)  -  COS (PSIO)*SIN(PHIO) 
A(8,10)=  VO*SIN(PSIO)*SIN(THETAO)*COS(PHIO)  -  VO*COS (PSIO)* . . . 

SIN(PHIO)  -  WO*SIN(PSIO)*SIN(THETAO)*SIN(PHIO)  -  ... 

WO*COS(PSIO)*COS(PHIO) 
A(8,ll)=  -UO*SIN(PSIO)*SIN(THETAO)  +  VO*SIN(PSI-0)*COS  (THETAO)* . 

SIN(PHIO)  +  WO*SIM(PSIO)*COS(THETAO)*COS(PHIO) 
A(8,12)=  UO*COS(PSIO)*COS(THETAO)  +  VO*COS (PSIO)*SIN(THETAO)* . , 

SIN(PHIO)  -  VO*SIN(PSIO)*COS(PHIO)  +  WO*COS (PSIO)* . . . 

SIN(THETAO)*COS(PHIO)  +  WO*SIN(PSIO)*SIN(PHIO) 

A(9,l)  =  -SIN(THETAO) 
A(9,2)  =  COS(THETAO)*SIN(PHIO) 
A(9,3)  =  COS(THETAO)*COS(PHIO) 

A(9,10)=  VO*COS (THETAO )*COS (PHIO )-WO*COS (THETAO )*SIN (PHIO) 
A(9,ll)=  -UO*COS(THETAO)-VO*SIN(THETAO)*SIN(PHIO)  -... 
WO*SIN(THETAO)*COS(PHIO) 

A(10,4)  =  1.0 

A(10,5)  =  SIN(PHIO)*TAN(THETAO) 
A(10,6)  =  COS(PHI0)*TAN(THETA0) 

A(10, 10)=  Q0*COS(PHI0)*TAN(THETA0)  -  RO*SIN(PHIO)*TAN(THETAO) 
A(10,ll)=  Q0*SIN(PHI0)/COS(THETA0)*1.0/COS(THETA0)  +  ... 
R0*COS (PHIO ) /COS (THETAO )  *1 . 0/COS (THETAO ) 

A(ll,5)  =  COS(PHIO) 

kill, 6}    =  -SIN(PHIO) 

A(ll,10)=  -QO*SIN(PHIO)  -  R0*COS(PHI0) 

A(12,5)  =  SIN(PHI0)/COS(THETA0) 
A(12,6)  =  COS(PHI0)/COS(THETA0) 

A  (12, 10)  =  QO*COS (PHIO) /COS (THETAO ) -RO*SIN(PHIO) /COS (THETAO) 
A(12,ll)=  Q0*SIN(PHIO)/COS(THETA0)*TAN(THETA0)  +  ... 
RO*COS (PHIO ) /COS (THETAO ) *TAN(THETAO ) 
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* 

*  WRITE (10, 200) ((A(I, J), J=l, 12), 1=1, 12) 

*  CALCULATE  THE  B  MATRIX 

B(l,l)  =  RHO/2*L**3*XRDR*U0*R0+RHO/2^**2MXRDR*U0*V0+U0**2*.., 

2*XDRDR*DR) 
B(l,2)  =  U0*Q0*XQDB/2  +  U0*W0*XWDB/2  +  U0**2*XDBDB*DBS 
B(l,3)  =  U0*Q0*XQDB/2  +  U0*W0*XWDB/2  +  U0**2*XDBDB*DBP 
B(l,4)  =  U0*Q0*XQDS  +  U0*W0*XWDS  +U0**2*2*XDSDS*DS+RHO/2*L**3* , 

XQDSN*UO*Q0*EPS  +  RHO/2*L**2*(XWDSN*U0*W0  +  2*XDSDSN*. 


U0**2*DS)*EPS 
1(1,5)  =  RHO/2*L** 
B(l,6)  =  SIN(THETAO) 


B(l,5)  =  RHO/2*L**2*0.12*CD0*0.12*RPM 


B(2,l)  =  RHO/2*L**2*YDR*U0**2 
B(2,6)  =  -COS(THETA0)*SIN(PHI0) 

-  U0**2*ZDB/2*RHO/2*L**2 

=  U0**2*ZDB/2*RHO/2*L**2 

=  U0**2*ZDS*RHO/2*L**2  +  RHO/2*L**2*ZDSN*U0**2*EPS 

=  -COS(THETA0)*COS(PHI0) 

=-RHO/2*L**3*U0**2*KDB/2 
=  RHO/2*L**3*U0**2*KDB/2 
=  -YB*COS(THETA0)*COS(PHI0)  +  ZB*COS(THETA0)*SIN(PHI0) 

B(5,2)  =  RHO/2*L**3*U0**2*MDB/2 

B(5,3)  =  RHO/2*L**3*U0**2*MDB/2 

B(5,4)  =  RHO/2*L**3*(U0**2*MDS+MDSN*U0**2*EPS). 

B(5,6)  =  XB*COS(THETA0)*COS(PHI0)  +  ZB*SIN(THETAO) 

B(6,l)  =  RHO/2*L**3*NDR*U0**2 

B(6,6)  =  -XB*COS(THETA0)*SIN(PHI0)  -  YB*SIN(THETAO) 


* 

*  WRITE(  9,300)((B(I,J),J=1,6),I=1,6) 

*  FORMULATE  THE  A  AND  B  MATRIX  FOR  STATE  SPACE  REPRESENTATION 

*  MULTIPLY  MMINV  AND  DF/DX 

DO  80  I  =1,6 
DO  70  J  =  1,6 
SUM  =0.0 
DO  60  K  =  1,6 

SUM  =  SUM  +  MMINV(I,K)*A(K,J) 
60  CONTINUE 

AA(I,J)  =  SUM 
70       CONTINUE 
80    CONTINUE 

*  MULTIPLY  MMINV  AND  DF/DZ 

DO  50  I  =  1,6 

DO  40  J  =  7,12 
SUM  =0.0 

DO  30  K  =  1,6 

SUM  =  SUM  +  MMINV(I,K)*A(K,J) 
30  CONTINUE 

AA(I,J)  =  SUM 
40       CONTINUE 
50    CONTINUE 


DO  5  I  =  7,12 
DO  6  J  =  1,12 
AA(I,J)  =  A(I,J) 
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6        CONTINUE 
5      CONTINUE 


WRITE(10,200)((AA(I,J),J=1, 12) ,1=1,12) 


200   FORMAT(  6E12.4) 
* 

* 

*  MULTIPLY  MMINV  AND  DF/DU 

DO  110  I  =  1,6 
DO  100  J  =  1,6 
SUM  =0.0 
DO  90  K  =  1,6 

SUM  =  SUM  +  MMINV(I,K)*B(K,J) 
90  CONTINUE 

BB(I,J)  =  SUM 
100      CONTINUE 
110    CONTINUE 

WRITE (  9,300)((BB(I,J),J=1,6),I=1,6) 

300   FORMAT(6E12.4) 
* 

DO  405  I  =  1,6 

READ  (2,401)(GKK(I,J),  J=l,21) 

405   WRITE(3,401)(GKK(I,J),  J=l,21) 

401    FORMAT (3E20. 10) 
* 

* 

DERIVATIVE 
NOSORT 

* 

*****LINEAR  MODEL****************************************************** 

*  WRITE(8,600) 

*00    FORMAT (14HENTERED  DERIV.) 

*  CALCULATE  BB*U   PART  OF  XDOT  =  AA*X  +  BB*U 
* 

DO  10  J  =  1,6 
SUM  =  0.0 
DO  15  K  =  1,6 

SUM  =  SUM  +  BB(J,K)*UMOD(K) 
15       CONTINUE 

XDOTU(J)  =  SUM 
10     CONTINUE 

*  CALCULATE  AA*X 

DO  21  J=  1,12 
SUM  =0.0 
DO  25  K  =  1,12 

SUM  =  SUM  +  AA(J,K)*X(K) 
25       CONTINUE 

XDOTX(J)  =  SUM 
21     CONTINUE 

*  CALCULATE  XDOT  =  AA*X  +  BB*U 

DO  31  J  =  1,6 

XDOT(J)  =  XDOTX(J)  +  XDOTU(J) 
31     CONTINUE 

DO  35  J  =  7,12 

XDOT (J)  =  XDOTX(J) 

35    CONTINUE 
* 

UDOTM  =  XDOT(l' 

VDOTM  =  XDOT (2 

WDOTM  =  XDOT(3' 

PDOTM  =  XDOT (4 1 
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* 
* 


QDOTM  =  XDOT(5 
RDOTM  =  XDOT(6 
XDOTM  =  XDOT(7' 
YDOTM  =  XDOT(8' 
ZDOTM  =  XDOT(9, 
PHMDOT=  JXDOT(10; 
THETMD=  XDOT(ll) 
PSMDOT=  XDOT(12) 
WRITE(8,600) 
INTEGRATE  XDOT  TO  GET  THE  STATE  VECTOR  X 


UM  =INTGRL(6.0, 
VM=  INTGRL (0.0, 
WM=  INTGRL (0.0, 
PM=  INTGRL (0.0, 
QM=  INTGRL (0.0, 
RM=  INTGRL (0.0, 
XPOSM  =  INTGRL ( 
YPOSM  =  INTGRL ( 
ZPOSM  =  INTGRL ( 
PHIM  =  INTGRL(0 
THETAM  =  INTGRL 
PSIM  =  INTGRL (0 


•■   UM 

:  VM 

■■   WM 

:  pM 
:  QM 

■  RM 

■  XPOSM 

■  YPOSM 
=  ZPOSM 

=  PHIM 
=  THETAM 
=  PSIM 


ZDEPTH  =  ZORD  -  X(9) 
THMANG  =  X(ll)*57.3 


udotm; 

VDOTM 
WDOTM 
PDOTM 
QDOTM 
RDOTM, 

0.0,  xdotm; 

0.0,  YDOTM 
0.0,  ZDOTM 

.o,  phmdot; 

(0.0,  THETMD) 
.0,  PSMDOT) 


DRM  =  UMODi 
DBSM=  UMODi 
DBPM=  UMODi 
DSM  =  UMODi 
DRPM=  UMODi 
DBOY=  UMOD< 


* 

******CONTROL  LAW******************************************************* 


* 


DBS  =  UMOD(2' 
DBP  =  UMOD(3' 
DS   =  UMOD (4' 


DBS  = 


DBP  = 


DS   = 


-(GKK( 

GKK(2, 

GKK(2, 

GKK(2, 

ZPOSM 

UMOD ( 3 

-(GKK( 

GKK(3, 

GKK ( 3 , 

GKK(3, 

ZPOSM 

UMOD ( 3 

-(GKK( 

GKK (4, 

GKK(4, 


2,1)*U 
5  *Q  + 
9)*ZPOS 
12)*PSI 


GKK  (2 
+  GKK 
1)*U 
*6  + 
*ZPOS 
12)*PSI 
+  GKK(3 
)  +  GKK 
4.1)*U 
5  *Q  + 
9)*ZPOS 


+  GKK(2.2)*V 

GKK(2,6)*R  + 

+  GKK(2,10) 

+  GKK(2,13) 

,16)*THETAM 

(2,19)*UMOD( 

+  GKK(3.2)*V 

GKK(3,6)*R  + 

+  GKK(3,10) 

+  GKK(3,13) 

,16)*THETAM 

(3,19)*UM0D( 

+  GKK(4.2)*V 

GKK(4,6)*R  + 

+  GKK(4,10) 


+  GKK 
GKK  (2 
*PHI  + 
*WM  + 
+  GKK( 
4)) 
+  GKK 
GKK  (3 
*PHI  + 
*WM  + 
+  GKK( 
4)) 
+  GKK 
GKK  (4 
*PHI  + 


(2.3)*W  +  GKK(2,4)*P  +... 
,7)*XPOS  +  GKK(2,8)*YPOS  +, 
GKK(2,11)*THETA  +   ... 
GKK(2,14)*QM  +  GKK(2,15)*., 
2,17)*UMOD[2)  +  GKK(2,18)*, 

(3,3)*W  +  GKK(3,4)*P  +. .. 
,7)*XPOS  +  GKK(3,8)*YPOS  +, 
GKK(3,11)*THETA  +   ... 
GKK(3.14)*QM  +  GKK(3,15)*., 
3,17)*UMOD[2)  +  GKK(3,18)*, 

(4.3)*W  +  GKK(4,4)*P  +. . . 
,7)*XPOS  +  GKK(4,8)*YP0S  +, 
GKK(4,11)*THETA  +   ... 
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GKK(4,12)*PSI  +  GKK(4,13)*WM  +  GKK(4,14)*QM  +  GKK(4,15)*. . . 
ZPOSM  +  GKK(4,16)*THETAM  +  GKK(4, 17 )*UMOD(2)  +  GKK(4,18)*.. 
UMOD(3)  +  GKK(4,19)*UM0D(4)) 

*  PUT  IN  STERN  AND  BOW  PLANE  STOPS 

IF(ABS(DBS).GT.0.6)  THEN 

DBS  =  0.6*ABS(DBS)/DBS 
END  IF 
IF(ABS(DBP).GT.0.6)  THEN 

DBP  =  0.6*ABS(DBP)/DBP 
END  IF 
IF(ABS(DS).GT.0.6)  THEN 

DS  =  0.6*ABS(DS)/DS 
END  IF 

******NON- LINEAR  MODEL************************************************ 
* 

*  PROPULSION  MODEL 
* 

SIGNU  =1.0 

IF  (U.LT.0.0)  SIGNU  =  -1.0 

IF  (ABS(U).LT.XITEST)  U  =  X1TEST 

SIGNN  =1.0 

IF  (RPM.LT.0.0)  SIGNN  =  -1.0 

ETA  =  0.012*RPM/U 

RE  =  U*L/NU 

CDO  =   .00385  +  (1.296E-17)*(RE  -  1.2E7)**2 

CT   =  0.008*L**2*ETA*ABS(ETA)/(A0) 

CT1  =  0.008*L**2/(A0) 

EPS  =  -1.0+SIGNN/SIGNU*(SQRT(CT+1.0)-1.0)/(SORT(CT1+1.0)-1.0) 

XPROP  =  CDO*(ETA*ABS(ETA)  -  1.0) 

* 

*  CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 

*  INTEGRATE  USING  A  4  TERM  GAUSS  QUADUTURE 

LATYAW  =0.0 
NORPIT  =0.0 
DO  500  K  =  1,4 

UCF(K)  =  SQRT((V+G4(K)*R*L)**2  +  (W-G4(K)*Q*L)**2) 

IF(UCF(K).GT.1E-10)  THEN 

TERMO   =  (RHO/2)*(CDY*HH(K)*(V+G4(K)*R*L)**2  +. . . 
CDZ*BR(K)*(W-G4(K)*Q*L)**2) 

TERM1   =  TERM0*(V+G4(K)*R*L)/UCF(K) 

TERM2   =  TERM0*(W-G4(K)*Q*L)/UCF(K) 

LATYAW  =  LATYAW  +  TERM1*GK4(K)*L 

NORPIT  =  NORPIT  +  TERM2*GK4(K)*L 

END  IF 
500   CONTINUE 

*  FORCE  EQUATIONS 

* 

*  surge  FORCE 

FP(1)  =  MASS*V*R  -  MASS*W*Q  +  MASS*XG*Q**2  +  MASS*XG*R**2- . . . 
MASS*YG*P*Q  -  MASS*ZG*P*R  +  (RHO/2)*L**4*(XPP*P**2  +... 
XQQ*Q**2  +  XRR*R**2  +  XPR*P*R)  +(RHO/2)*L**3*(XWQ*W*Q  +. 
XVTJ*V^P+XVR*V*R+U*Q*(XQDS*DS+XQDB/2*DBP)+XRDR*U*R*DR)  +  . . 
(RHO/2)*L**2*(XVV*V**2  +  XWW*W**2  +  XVDR*U*V*DR  +  U*W*.. 
(XWDS*DS+XWDB/2*DBP)+U**2*(XDSDS*DS**2+XDBDB/2*DBP**2+. . 
XDRDR*DR**2))- (WEIGHT  -BOY)*SIN(THETA)  +(RHO/2)*L**3*  .. 
XQDSN*U*Q*DS*EPS+ (RHO/  2 )  *L**2*  (XWDSN*U*W*DS+XDSDSN*U**2* 
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DS**2)*EPS  +(RHO/2)*L**2*U**2*XPROP+RHO/2*L**3*U*Q*  ... 
XQDB/2*DBS  +RHO/2*L**2*U**2*XDBDB/2*DBS**2+  . . . 
RH0/2*L**2*XWDB/2*DBS*U*W 

*  sway  FORCE 

FP(2)  =  -MASS*U*R  +  MASS*XG*P*Q  +  MASS*YG*R**2  -  MASS*ZG*Q*R  + 
(RHO/2)*L**4*(YPQ*P*Q  +  YQR*Q*R)+(RHO/2)*L**3*(YP*U*P  + 
YR*U*R  +  YVQ*V*Q  +  YWP*W*P  +  YWR*W*R)  +  (RHO/2)*L**2* 
(YV*U*V  +  YVW*V*W  +YDR*U**2*DR)  -LATYAW  +  (WEIGHT-BOY)  * . 
COS(THETA)*SIN(PHI) 

*  heave  FORCE 
* 

FP(3)  =  MASS*U*Q  -  MASS*V*P  -  MASS*XG*P*R  -  MASS*YG*Q*R  + 

MASS*ZG*P**2  +  MASS*ZG*Q**2  +  (RH0/2)*L**4*(ZPP*P**2  +. 
ZPR*P*R  +  ZRR*R**2)  +  (RHO/2)*L**3*(ZQ*U*Q  +  ZVP*V*P  +. 
ZVR*V*R)  +(RHO/2)*L**2*(ZW*U*W  +  ZW*V**2  +  U**2*(ZDS*. 
DS+ZDB/2*DBP))-NORPIT+ (WEIGHT-BOY) *COS (THETA)*COS (PHI )+ 
(RHO/2)*L**3*ZQN*U*Q*EPS  +(RHO/2)*L**2*(ZWN*U*W  +ZDSN*. 
U**2*DS)*EPS+  RHO/2*L**2*U**2*ZDB/2*DBS 

*  ROLL  FORCE 

FP(4)  =  -IZ*Q*R  +IY*Q*R  -IXY*P*R  +IYZ*0**2  -IYZ*R**2  +IXZ*P*Q  + 
MASS*YG*U*Q  -MASS*YG*V*P  -MASS*ZG*W*P+(RHO/2)*L**5*(KPQ* 
P*Q  +  KQR*Q*R)  +(RHO/2)*L**4*(KP*U*P  +KR*U*R  +  KVQ*V*Q  + 
KWP*W*P  +  KWR*W*R)  +(RHO/2)*L**3*(KV*U*V  +  KVW*V*W)  +  . 
(YG*WEIGHT  -  YB*BOY)^COS(THETA)*COS(PHI)  -  (ZG*WEIGHT  - 
ZB*BOY)*COS(THETA)*SIN(PHI)  +  (RHO/2)*L**4*KPN*U*P*EPS  + 
(RHO/2)*L**3*U**2*KPROP  +MASS*ZG*U*R+  ... 
•  RHO/2*L**3*U**2*(KDB/2*DBP-KDB/2*DBS) 

*  PITCH  FORCE 

FP(5)  =  -IX*P*R  +IZ*P*R  +IXY*Q*R  -IYZ*P*Q  -IXZ*P**2  +IXZ*R**2  - 
MASS*XG*U*Q  +  MASS*XG*V*P  +  MASS*ZG*V*R  -  MASS*ZG*W*Q  +. 
(RHO/2)*L**5*(MPP*P**2  +MPR*P*R  +MRR*R**2)+(RHO/2)*L**4* 
(MQ*U*6  +  MVP*V*P  +  MVR*V*R)  +  (RHO/2)*L**3^(MW*U*W  +  .. 
MVV*V**2+U**2*(MDS*DS+MDB/2*DBP))+  NORPIT  -(XG*WEIGHT-  . 
XB*BOY)*COS(THETA)*COS(PHI)  +... 

(RHO/2)*L**3*(MWN*U*W+MDSN*U**2*DS)*EPS+  RHO/2*L**3*. . . 
U**2*MDB/2*DBS-(ZG*WEIGHT-ZB*BOY)*SIN(THETA) 

•k 

*  YAW  FORCE 

FP(6)  =  -IY*P*Q  +IX*P*Q  +IXY*P**2  -IXY*Q**2  +IYZ*P*R  -IXZ*Q*R  - 
MASS*XG*U*R  +  MASS*XG*W*P  -  MASS*YG*V*R  +  MASS*YG*W*Q  +. 
(RHO/2)*L**5*(NPQ*P*Q  +  NQR*Q*R)  +(RHO/2)*L**4*(NP*U*P+ . 
NR*U*R  +  NVQ*V*Q  +NWP*W*P  +  NWR*W*R)  +(RHO/2)*L**3*(NV* . 
U*V  +  NVW*V*W  +  NDR*U**2*DR)  -  LATYAW  +  (XG*WEIGHT  -  .  . 
XB*BOY)*COS(THETA)*SIN(PHI)+(YG*WEIGHT)*SIN(THETA)... 
+ (RHO/2 ) *L**3*U**2*NPROP-YB*BOY*SIN (THETA) 

*  IF(Z.EQ.50.0)THEN 

*  WRITE  (8,500)(FP(I),  I  =  1,6) 

*  Z  =  0.0 

*  END  IF 
* 

*  NOW  COMPUTE  THE  F(l-6)  FUNCTIONS 

DO  600  J  =  1,6 

F(J)  =  0.0 
DO  600  K  =  1,6 

F(J)  =  MMINV(J,K)*FP(K)  +  F(J) 

600   CONTINUE 
* 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 

*  FIRST  SET  THE  DRIFT  CURRENT  VALUES 
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* 


UCO  =0.0 
VCO  =0.0 
WCO  =0.0 


*     INERTIAL  POSITION  RATES  F(7-9) 


F(7)  =  UCO  +  U*COS(PSI)*COS(THETA)  +  V*(C0S(PSI )*SIN(THETA)* . . . 
SIN(PHI)  -  SIN(PSI)*COS(PHlV 
COS(PHI)  +  SIN(PSI)*SIN(PHI) 


SIN(PHI)  -  SIN(PSI)*COS(PHI))  +  W*(COS (PSI)*SIN(THETA)* 


F(8)  =  VCO  +  U*SIN(PSI)*COS(THETA)  +  V*(SIN(PSI )*SIN(THETA)* . . . 

SIN(PHI)  +  C0S(PSI)*C0S(PHI))  +  W*(SIN(PSI )*SIN(THETA)* . . . 
COS(PHI)  -  COS(PSI)*SIN(PHI)) 

•k 

F(9)  =  WCO  -  U*SIN(THETA)  +V*COS (THETA) *SIN(PHI)  +W*COS (THETA)* . . . 
COS (PHI) 

*     EULER  ANGLE  RATES  F( 10-12) 


F(10)  =  P  +  Q*SIN(PHI)*TAN(THETA)  +  R*COS(PHI)*TAN(THETA) 

F(ll)  =  Q*COS(PHI)  -  R*SIN(PHI) 

F(12)  =  Q*SIN(PHI)/COS(THETA)  +  R*COS (PHI)/COS (THETA) 


*  IF  (Z.EQ.l.O)WRITE  (9 , 500) (F(I) ,  I  =  1,12) 
*00    FORMAT(6E12.4) 

*  Z  =  Z  +  1 


* 

DYNAMIC 

* 


UDOT  =  F(l 
VDOT  =  F(2 
WDOT  =  F(3' 
PDOT  =  F(4' 
QDOT  =  F(5 
RDOT  =  F(6' 
XDOTA=  F(7 
YDOT  =  F(8' 
ZDOT  =  F(9' 
PHIDOT  =  F'< 
THETAD  =  Fi 
PSIDOT  =  Fi 

U  =  INTGRL (6.0, UDOT ' 
V  =  INTGRL (0.0, VDOT , 
W  =  INTGRL (0.0, WDOT 
P  =  INTGRL (0.0, PDOT' 
Q  =  INTGRL (0.0, QDOT' 
R  =  INTGRL ( 0.0, RDOT ! 
XPOS  =  INTGRL (0.0,XDOTA) 
YPOS  =  INTGRL (0.0, YDOT) 
ZPOS  =  INTGRL (0.0, ZDOT) 
PHI  =  INTGRL(0.0, PHIDOT) 
THETA  =  INTGRL (0.0, THETAD) 
PSI  =  INTGRL(0.0, PSIDOT) 

ZNEW  =  -ZPOS 

PHIANG  =  PHI/0.0174532925 
THEANG  =  THETA/0. 0174532925 
PSIANG  =  PSI/0. 0174532925 


IF  (IFLAG.EQ.O.AND.JFLAG.EQ.O)   THEN 
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UM0D(4)=  15.0*0.0174532925 
UMOD(3)  =  -15.0*0.0174532925 
UMOD(2)  =  -15.0*0.0174532925 
END  IF 

IF (IFLAG.EQ.O. AND. ABS (THMANG). GT. 37.0)   THEN 
ZCHG  =  X(9)   -  5.0 
I FLAG  =  I FLAG  +  1 
ENDIF 
IF  (IFLAG.GT.0.0. AND. JFLAG. EQ.O)  THEN 

UM0D(4)=  2.05*0.0174532925 

UM0D(2)  =  0.0 

UM0D(3)  =  0.0 
ENDIF 
IF  (IFLAG.GT.O.AND.ZCHG.GT.ZDEPTH)   THEN 

UM0D(4)=  -11.0*0.0174532925 

UM0D(3)  =  11.0*0.0174532925 

UM0D(2)  =  11.0*0.0174532925 
ENDIF 
IF  (ZDEPTH.LT.3.0.AND.ABS(THMANG).LT.4.10)   THEN 

UM0D(4)=  0.0 

UM0D(3)  =  0.0 

UM0D(2)  =0.0 

JFLAG  =  JFLAG  +  1 
ENDIF 
IF  ( JFLAG. GT.O)  THEN 

UM0D(4)=  0.0 

UM0D(3)  =  0.0 

UM0D(2)  =  0.0 
ENDIF 


CONTROL  FINTIM  =200.00,DELT  =  .01 

SAVE   .20,XPOS,XPOSM,U,UM,ZPOS,ZPOSM,W,WM,DBPM,  .  .  . 

DBS , DBSM , DS , DSN , THEANG , THMANG , Q , QM 
PRINT  2.0,  XPOS  ,  XPOSM ,  U ,  UM ,  ZPOS  ,  ZPOSM ,  W ,  WM ,  DBPM 

DBS , DBSM , DS , DSM , THEANG , THMANG , Q , QM 
GRAPH  (G1,DE=TEK618)  TIME(NI=10,UN=SEC)  ZPOS(LI=l ,UN=FT) . . . 

ZPOSM(LI=2,UN=FT) 
GRAPH  (G2,DE=TEK618)  TIME (NI=10 ,UN=SEC)  W(LI=1 ,UN= ' FT/SEC ') . 

WM(LI=2,UN=' FT/SEC') 
GRAPH  (G3,DE=TEK618)  TIME(NI=10,UN=SEC)  Q(LI=1 ,UN= ' RAD/SEC ' ) 

QM(LI=2,UN=' RAD/SEC) 
GRAPH  (G4,DE=TEK618)  TIME(NI=10 ,UN=SEC)  THEANG ( LI=1 ,UN=DEG) . 

THMANG(LI=2,UN=DEG) 
XPOS(UN=FT)  ZPOS(UN=FT) 
XPOSM(UN=FT)  ZPOSM(UN=FT) 
TIME(NI=10,UN=SEC)  DBS (LI=1 ,UN=RADIANS) 
DBSM(LI=2,UN=RADIANS) 
GRAPH  (G8,DE=TEK618)  TIME(NI=10 ,UN=SEC)  DS  (LI=1 ,UN=RADIANS) 

DSM (LI=2 , UN=RADIANS ) 
'DEPTH  VS  TIME) 
HEAVE  VS  TIME) 
PITCH  RATE  VS  TIME) 
PITCH  ANGLE  VS  TIME) 
ACTUAL  DIVE  PROFILE) 
MODEL  DIVE  PROFILE) 
BOW  PLANE  ANGLE) 
STERN  PLANE  ANGLE) 


GRAPH  (G5,DE=TEK618 
GRAPH  (G6,DE=TEK618 
GRAPH  (G7,DE=TEK618 


LABEL 

LABEL 

LABEL 

LABEL 

LABEL 

LABEL 

LABEL 

LABEL 

END 

STOP 


G1,DE=TEK618' 
G2,DE=TEK618 

,G3,DE=TEK618 
G4,DE=TEK618 
G5,DE=TEK618 
G6,DE=TEK618' 
G7,DE=TEK618' 

iGS^E^EKeiS. 
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******5UBR0UTINE  ET&T**************************************************** 

************************************************************************* 

PROGRAM   ETAT 

IMPLICIT  REAL*8(A-H.O-Z) 

COMMON/ SYST/A (40, 40) ,B(40,40) ,C(40,40) ,D(40,40) 

COMMON/STATES/X(40) ,Y(40) ,U(40) ,W(40) 

COMMON/DIM/N , M , NR , NKS , EPS 

C0MM0N/DIM1/DT 

COMMON/ FLAGS/ I OPT ( 5 ) 

COMMON/OPTIM/Q ( 24 , 24 ) , R ( 24 , 24 ) 
C     DATA  EPS/1. OE-7/ 
C 

C      INITIALIZE  ALL  SYSTEM  MATRICES 
C 

EPS  =  1.0E-7 

DO  10  1=1,40 

X(I)=0.0 

Y(I)=0.0 

U(I)=0.0 

10  CONTINUE 

DO  15  1=1,40 

DO  15  J=l,40 

A(I,J)=0.0 

B(I,J)=0.0 

C(I,J)=0.0 

D(I,J)=0.0 

15  CONTINUE 

DO  16  1=1,24 
DO  16  J=l,24 
Q(I,J)=0.0 
R(.I,J)=0.0 

16  CONTINUE 
C 

C     SET  UP  COEFICIENT  MATRICES , INPUTS , INITIAL  CONDITIONS 
C 

CALL  INPUT 

CALL  MTXEXP 

CALL  ROOTS 

DO  20  K=1,NKS 

CALL  EXCIT(K) 

CALL  UPDAT(K) 

CALL  POUT(K) 
20    CONTINUE 

IF  (IOPT(l).EQ.l)    CALL  OPTIMA 
C      IF  (IOPT(l).EQ.l)    CALL  POUTOP 

END 
******suBROUTINE  INPUT************* A************'1:***********^*^********** 

■k-k-k-k-k-k-k-k-k-k-k'k-k-k-k^'k-k-k  ***********************************  ******************* 

SUBROUTINE  INPUT 

IMPLICIT  REAL*8(A-H.O-Z) 

COIJIMON/SYST/A(40,40),B(40,40),C(40,40),D(40,40) 

COMMON/STATES/X(40) ,Y(40) ,U(40) ,W(40) 

COMMON/DIM/N ,M,NR, NKS, EPS 

COMMON/DIM1/DT 

COI-IMON/ FLAGS/ 1  OPT  ( 5 ) 

COMMON/OPTIM/Q ( 24 , 24 ) , R ( 24 , 24 ) 
C     OPEN(UNIT=5 , FILE= ' FILE ' , STATUS= ' OLD ' ) 
C     OPEN(UNIT=6,FILE='FILEl ,STATUS= ' OLD ' ) 

READ  (5,10)   N,L,M,K,NKS,IOPT(l),DT 

WRITE (6, 10)   N,L,M,K,NKS,IOPT(l),DT 

READ  (5,9)   NAS 

WRITE (6, 9)   NAS 

DO  11  11=1. NAS 

READ  (5,25)  I,J,A(I,J) 

WRITE(6,25)  I,J,A(I,J) 

11  CONTINUE 

READ  (5,9)  NBS 
WRITE (6, 9)  NBS 
DO  12  11=1, NBS 
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READ  (5,25)  I,J,B(I,J) 
WRITE(6,25)  I,J#B(I,J) 

12  CONTINUE 

READ  (5,9)   NCS 

WRITE (6, 9)   NCS 

DO  13  11=1, NCS 

READ  (5,25)  I,J,C(I,J) 

WRITE(6,25)  I,J,C(I,J) 

13  CONTINUE 

READ  (5,9)   NDS 

WRITE (6, 9)   NDS 

DO  14  11=1, NDS 

READ  (5,25)  I,J,D(I,J) 

WRITE(6,25)  I,J,D(I,J) 

14  CONTINUE 
READ(5,9)  NXS 
WRITE (6, 9)  NXS 
DO  35  11=1, NXS 
READ(5,25)  I,J,X(I) 
WRITE(6,25)  I,J,X(I) 

35    CONTINUE  ■ 

IF(IOPT(l).NE.l)   GO  TO  190 

READ(5,9)   NQS 

WRITE (6, 9)  NQS 

DO  150  11=1, NQS 

READ(5,25)  I,J,Q(I,J) 

WRITE(6,25)  I,J,Q(I,J) 
150   CONTINUE 

READ(5,9)   NRS 

WRITE (6, 9)  NRS 

DO  180  11=1, NRS 

READ(5,25)   I,J,R(I,J) 

WRITE(6,25)  I,J,R(I,J) 
180   CONTINUE 

NR=L 

9  FORMAT(5X,I5) 

10  FORMAT(5X,6I5,E10.4) 
25  FORMAT  (15 , 15 ,E10 .4) 
190    RETURN 

END 
******5UBROUTINE  ^TXEXP************************************************** 

SUBROUTINE  MTXEXP 

IMPLICIT  REAL*8(A-H.O-Z) 

COMMON/SYST/A(40,40),B(40,40),C(40,40),D(40,40) 

COMMON/DIM/N,M,NR,NKS,EPS 

COMMON/DIM1/DT 

COMMON/ETT/E(40,40) ,H(40.40) 

DIMENSION  DD(40,40),L(50),RHO(50,2),W(50) 

MK=30 

DO  20  1=1, N 

L(I)=1 

RHO(I,1)=1.0 

DO  20  J=1,N 

IF(I.EQ.J)     GO  TO  10 

E(I,J)=0.0 

DD(I,J)=0.0 

H(I,J)=0.0 
GO  TO  20 
10     CONTINUE 

E(I,J)=1.0 

DD(I,J)=1.0 

H(I,J)=DT 
20    CONTINUE 

MM=0 

K=l 

X=DT 
30     CONTINUE 

DO  80  1=1, N 

IF(L(I).EQ.O)  GO  TO  80 
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DO  40  J=1,N 

W(J)=0.0 
40    CONTINUE 

DO  60  KK=1,N 

Y=DD(I,KK) 

IF(Y.EQ.O.O)   GO  TO  60 

DO  50  J=1,N 

W(J)=W(J)+Y*A(KK,J) 
50     CONTINUE 
60     CONTINUE 

DO  70  J=1,N 

DD(I,J)=W(J)*X 
70     CONTINUE 
80     CONTINUE 

K=K+1 

X=K 

X=DT/X 

DO  100  1=1, N 

IF(L(I).EQ.O)   GO  TO  100 

Y1P=0.0 

DO  90  J=1,N 

E(I,J)=E(I,J)+DD(I,J) 

H(I,J)=H(I,J)+X*DD(I/J) 

Y1P=Y1P+ABS(E(I,J)) 
90     CONTINUE 

RH0(I,2)=Y1P 

IF(ABS((RHO(I,2)-RHO(I,l))/RHO(I/2)).GT.EPS)   GO  TO  100 

L(I)=0 

MM=MM+1 
100   CONTINUE 

IF(K.GT.MK)    GO  TO  130 

IF(MM.EQ.N)   GO  TO  120 

DO  110  1=1 ,N 

RHO(I,l)=RHO(I,2) 
110    CONTINUE 

GO  TO  30 
120    CONTINUE 

DO  125  I=1,N 

DO  125  J=1,N 

DD(I,J)=0.0 

DO  125  K=1,N 

DD(I,J)=DD(I,J)+H(I,K)*B(K,J) 
125   CONTINUE 

DO  135  1=1, N 

DO  135  J=1,N 

H(I,J)=DD(I,J) 
135   CONTINUE 

WRITE  (6,190) 
190    FORMAT (5X, 'P-MATRIX' ./) 

WRITE (6, 200)  ((E(I,J),J=1,N),I=1,N) 

WRITE  (6,195) 
195   FORMAT ( 5X, 'QB-MATRIX' ,/) 

WRITE (6, 200)  ((H(I,J),J=1,N),I=1,N) 
200   FORMAT (6E 12. 4) 

RETURN 
130    CONTINUE 

WRITE(6,140)   MK  ,EPS 

STOP 
140    FORMAT (IX, 'MATRIX  EXPONENTIAL  FAILED  TO  CONVERGE  AFTER   ',14, 
1'  ITERATIONS' ,/, IX, 'CONVERGENCE  FACTOR ' ,E12 .4) 

END 
******SUBROUTINE  ROOTS*************************************************** 

SUBROUTINE  ROOTS 

IMPLICIT  REAL*8(A-H,0-Z) 

COMPLEX*16   ZZ 

COMMON/SYST/A(40,40) ,B(40 ,40) , C(40 ,40) ,D(40,40) 

COMMON/DIM/N,M,NR,NKS,EPS 

COMMON/DIM1/DT 

DIMENSION  W(80) ,ZZ(40,40) ,WK(3200) 

127 


DIMENSION  XX(40,40) ,RZ(3200) 
EQUIVALENCE  (ZZ(1,1),  RZ(l)) 
IJOB=2 
IZ=40 
IA=40 

DO  10  1=1,3200 
WK(I)=0.0 
10    CONTINUE 

DO  15  1=1,80 

w(i)=o.o 

15    CONTINUE 

DO  20  1=1,40 

DO  20  J=l,40 

ZZ(I,J)=0.0 
20    CONTINUE 

DO  25  1=1, N 

DO  25  J=1,N 

XX(I,J)=A(I,J) 
25    CONTINUE 

DO  4  I  =  1,N 
4     WRITE(6,3)(XX(I,J),  J=1,N) 
3     FORMAT(6E12.4) 

CALL  EIGRF(XX,N,IA,IJOB,W.RZ,IZ,WK,IER) 

WRITE(6,8)(W(I),  I  =  1,80) 
8     FORMAT(4E12.4) 

N2=N*2 

DO  30  1=1, N2, 2 

W1=W(I) 

11=1  +  1 

W2=W(I1) 

I2=(I+l)/2 
30    WRITE(6,100)  I2,W1,W2 

DO  50  1=1, N 

DO  40  J=1,N 

XX(1,J)=REAL(ZZ(J,I)) 

XX(2,J)=DIMAG(ZZ(J,I)) 
40    WRITE  (6,120)  I,XX(1, J) ,XX(2, J) 
50    CONTINUE 
100    FORMAT (5X, 'EIGENVALUES' ,10X, 'REAL  PART', 

110X, 'IMAGINARY  PART' , / , 5X, 13 , 12X,E12 .4, 10X,E12 .4) 
120    FORMAT(5X, 'EIGENVECTORS' , 15 , 5X,2E12 .4) 
130    FORMAT ( 5X, ' IER  AND  PERFORMANCE  INDEX' , 15 , 10X,E12 .4) 

WRITE  (6,130)   IER,WK(1) 

RETURN 

END 
******SUBROUTINE  EXCIT(K) ************************************************ 

SUBROUTINE  EXCIT(K) 
IMPLICIT  REAL*8(A-H.O-Z) 

COMMON/SYST/A(40,40),B(40,40),C(40,40),D(40,40) 
COMMON/ STATES/X( 40) ,Y(40) ,U(40) ,W(40) 
COMMON/DIM/N , M , NR , NKS , EPS 
COMMON/DIM1/DT 
T=DT*FLOAT(K) 
U(1)=0.0 
RETURN 
END 
****A*5UBROUTINE  UPDAT   (k)**************************************^******* 

SUBROUTINE  UPDAT (K) 

IMPLICIT  REAL*8(A-H.O-Z) 

COMMON/SYST/A(40,40),B(40,40),C(40,40),D(40,40) 

COMMON/STATES/X(40),Y(40),U(40),W(40) 

COMMON/DIM/N, M,NR, NKS, EPS 

COMMON/DIM1/DT 

COMMON/ETT/E(40.40) ,H(40,40) 

DIMENSION  XS(40),XN(40),YS(4O),YN(40) 

T=DT*FLOAT(K) 

DO  10  1=1, N 

XS(I)=0.0 
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+E< 

;i,j)*x(j) 

+H< 

I, J)*U(J) 

+C< 

I,J)*X(J) 

+D< 

I,J)*U(J) 

XN(I)=0.0 

YS(I)=0.0 

YN(I)=0.0 

DO  10  J=1,N 

XS(I)=XS(I' 

XN(I)=XN(I 

YS(I)=YS(I 

YN(I)=YN(I 
10     CONTINUE 

DO  20  1=1. N 

X(I)=XS(I)+XN(I 

Y(I)=YS(l)+YN(I 
20    CONTINUE 

RETURN 

END 
******5UBROUTINE  pguT(K) ************************************************* 

**********  *************************************************************** 

SUBROUTINE  POUT(K) 

IMPLICIT  REAL*8(A-H.O-Z) 

COMMON/SYST/A(40,40),B(40,40),C(40,40),D(40,40) 

COMMON/STATES/X(40)  ,Y(40)  ,U(40)  ,W(40) 

COMMON/DIM/N , M , NR , NKS , EPS 

COMMON/DIM1/DT 

T=FLOAT(K)*DT 

IF(K.EQ.l)   WRITE  (6,110) 

WRITE  (6,100)    T,Y(1),Y(2),Y(3),Y(4) 
100   FORMAT(5X,F10.2,5X,4E12.4) 

110    FORMAT('TIME' ,5X, 'XI' ,5X, ' X2 ' ,5X, ' X3 ' ,5X, '  X4  ■ ,5X, ' Ul ' 
1,10X,'U2\/) 

RETURN 

END 
******SUBROUTINE  OPTIMA************************************************** 

*  THIS  SUBROUTINE  SETS  UP  THE  SYSTEM  AND  ADJOINT  EQUATIONS     * 

*  MATRICES  AS  * 

*  A   -(B(R-l)BT)  * 

*  SS  =  * 

*  -Q         _AT  * 

*  AND  FINDS  THE  EIGENVALUES  /EIGENVECTORS  OF  SS .  * 

*  COLLECTING  THE  STABLE  VECTORS  AS  IN  POTTERS  METHOD  * 

*  AND  PARTITIONING, RESULTS  IN  THE  SOLUTION  OF  THE  * 

*  RICCATI  EQUATION  FOR  THE  OPTIMUM  STATE  FEEDBACK  * 

*  GAINS. THIS  ROUTINE  LIMITS  A(24,24).  * 
************************************************************************* 

SUBROUTINE  OPTIMA 

IMPLICIT  REAL*8(A-H,0-Z) 

COMPLEX*16  ZZ,W12,W22 

C0MMON/SYST/A(40, 40) .B(40,40).C (40,40 ),D(40,40) 

C0MM0N/0PTIM/Q(24,24) ,R(24,24) 

COMMON/DIM/N , M , NR , NKS , EPS 

COMMON/DIM1/DT 

DIMENSION   SS(48,48) ,TEMP(24,24) ,ZZ(48,48) ,W(96) ,WK(2400) 

DIMENSION  W12(24,24) ,W22(24,24) ,RZ(4608) 

EQUIVALENCE  (ZZ(1 , 1) ,RZ(1 ) ) 

IA=48 

IZ=48 

IJOB=l 

N2=2*N 

N4=2*N2 

N21=N2+1 

DO  1  1=1, N2 

DO  1  J=1,N2 
1     SS(I,J)=0.0 

DO  5  1=1, N 

DO  5  J=1.N 
5     TEMP(I,J)=0.0 
C     WRITE(6,140)    ((R(I,J),J=1,NR),I=1,NR) 

NDIM1=24 

NDIM2=48 

CALL  INVERT (R , DET , NR , NDIM1 , NDIM2 ) 
C     WRITE(6,150)   ((R(I/J)/J=1,NR),I=1,NR) 
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DO  10  1=1, NR 

DO  10  J=1,N 

DO  10  K=1,NR 
10    TEMP(I,J)=TEMP(I,J)+R(I,K)*B(J,K) 

DO  20  1=1, N 

DO  20  J=1,N 

JJ=J+N 

DO  20  K=1,NR 
20    SS(I,JJ)=SS(I,JJ)-B(I,K)*TEMP(K,J) 

DO  30  1=1, N 

DO  30  J=1,N 
30    SS(I,J)=A(I,J) 

DO  40  1=1, N 

DO  40  J=1,N 

II=I+N 

JJ=J+N 

SS(II,JJ)=-A(J,I) 
40    SS(II,J)=-Q(I,J) 

CALL  EIGRF(SS,N2,IA,IJOB,W,RZ,IZ,WK,IER) 

WRITE (6, 90) 

DO  50  1=1, N4, 2 

I2=(I+l)/2 

W1=W(I) 

11=1+1 

W2=W(I1) 
50    WRITE(6,120)   I2,W1,W2 
C     WRITE(6,100) 

DO  70  J=1,N2 

DO  60  1=1, N2 

SS(I,1)=REAL(ZZ(I,J)) 

SS(I,2)=DIMAG(ZZ(I,J)) 
CO    WRITE  (6,120)  J,SS(I , 1) ,SS(I ,2) 
60     CONTINUE 
70    CONTINUE 

C     COLLECT  ALL  STABLE  EIGENVECTORS  INTO  A  V-MATRIX 
C      (USING  SS( 48, 48)), PARTITION, AND  SOLVE  FOR  THE 
C     SOLUTION  OF  THE  RICCATTI  EQUATION  . 

DO  210  IC=1,N4,2 

JC=(IC+l)/2 

IF(W(IC).GE.0.0)    GO  TO  210 

J=J+1 

DO  200  1=1, N 

IPN=I+N 

W12(I,J)=ZZ(I,JC) 

W22(I,J)=ZZ(IPN,JC) 
200    CONTINUE 
210   CONTINUE 
C  INVERT  COMPLEX  W12(N,N) 

DO  220  1=1, N 

DO  220  J=1,N 

IPN=I+N 

JPN=J+N 

SS(I,J)=REAL(W12(I,J)) 

SS(IPN,J)=DIMAG(W12(I,J)) 

SS(I,JPN)=-SS(IPN,J) 

SS(IPN,JPN)=SS(I,J) 
220   CONTINUE 

NDIM1=48 

NDIM2=96 

CALL  INVERT(SS,DET,N2,NDIH1,NDIM2) 
C  FORM  W22*(W12)-1=P 

DO  230  1=1, N 

DO  230  J=1,N 

IPN=I+N 

Q(I,J)=SS(I,J) 
230   R(I,J)=SS(IPN,J) 

DO  240  1=1, N 

DO  240  J=1,N 

SS(I,J)=0.0 
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DO  240  K=1,N 

SS(I,J)=SS(I,J)+REAL(W22(I,K))*Q(K,J)-DIMAG(W22(I,K))*R(K,J) 
240   CONTINUE 
C  FORM  GAIN  MATRIX  INTO  THE  Q  ARRAY 

DO  250  1=1 ,NR 

DO  250  J=1,N 

Q(I,J)=0.0 

DO  250  K=1,N 

Q(I,J)=Q(I,J)+TEMP(I,K)*SS(K,J) 
250   CONTINUE 
C  COMPUTE  THE  CLOSED  LOOP  A-MATRIX 

DO  260  1=1, N 

DO  260  J=1,N 

DO  260  K=1,NR 
260   A(I,J)=A(I,J)-B(I,K)*Q(K,J) 

WRITE(6,270) 

DO  265  K=1,NR 
265   WRITE(2,275)   (Q(K, J) , J=l ,N) 
C     WRITE(6,280) 
C     DO  285  1=1, N 

C85   WRITE(6,275)    (A(I , J) , J=l ,N) 
C     CALL  ROOTS 

90     FORMAT (5X, ' EIGENVALUES-SYSTEM+ADJOINT- ' ) 
100    FORMAT(5X,  'EIGENVECTORS   RE/IMAG') 
120   FORMAT(5X,I5,10X,E12.4,10X,E12.4) 
150   FORMAT(5X, 'R-INVERSE' ,/,4E12.4) 
140   FORMAT(5X, 'R-MATRIX' ,/,4E12.4) 

270    FORMAT (5X, 'TOTAL  STATE  FEEDBACK  GAIN  MATRIX',/) 
275   FORMAT (3E 20. 10) 
280    FORMAT (5X, 'CLOSED  LOOP  A-MATRIX',/) 

RETURN 

END 

******SUBROUTINE  jiwERT************************************************** 
************************************************************************* 

SUBROUTINE  INVERT (A , DET . N , NDIM1 , NDIM2 ) 

IMPLICIT  REAL*8(A-H,0-Z) 
C        THIS  ROUTINE  INVERTS  A  SQUARE  MATRIX  USING 
C        GAUSS  ELIMINATION. THE  ORIGINAL  MATRIX  IS  DESTROYED 
C        AND  ITS  INVERSE  IS  RETURNED  AS  'A'. 

DIMENSION  A(NDIM1,NDIM2) 

NDIGIT=30 

SUM=0.0 

DO  10  1=1, N 

DO  10  J=1,N 

SUM=SUM+ABS(A(I,J)) 
10     CONTINUE 

SUM=10 . 0** ( -NDIGIT/2 . )*SUM/N**2 

NP1=N+1 

NPN=N+N 

DO  20  1=1, N 

IPN=I+N 

DO  20  J=NP1,NPN 

A(I,J)=0.0 

IF  (IPN.EQ.J)   A(I,J)=1.0 
20     CONTINUE 
C     DO  25  1=1, N 

C     WRITE(6,900)  (A(I , J) , J=l ,NPN) 
25    CONTINUE 

DET=1. 

INTCH=0 

DO  90  I=1,N 

IP1=I+1 

IF  (I.EQ.N)   GO  TO  50 

M=I 

DO  30  J=IP1,N 

IF  (ABS(A(M,I)).LT.ABS(A(J,I)))   M=J 
30     CONTINUE 

IF  (M.EQ.I)   GO  TO  50 

INTCH=INTCH+1 

DO  40  J=1,NPN 

131 


40 
50 


60 


70 
80 
90 


100 
C 
C 
26 

110 


120 

130 
140 
900 
910 


TEMP=A(M,J) 
A?M,J)=A(I,J) 
A(I,J)=TEMP 
CONTINUE 
CONTINUE 

IF(A(I,I).EQ.0.0)    GO  TO  110  - 
IF  (ABS(A(I,I)).LT.SUM)    WRITE(6,140) 
DO  60  J=IP1,NPN 
A(I,J)=A(I,J)/A(I,I) 
CONTINUE 
DO  80  J=1,N 
IF  (J.EQ.I)   GO  TO  80 
DO  70  K=IP1,NPN 
A(J,K)=A(J,K)-A(J,I)*A(I,K) 
CONTINUE 
A(J,I)=0.0 
CONTINUE 
DET=DET*A(I,I) 
CONTINUE 

DET=(-1)**INTCH*DET 
DO  100  1=1, N 
DO  100  J=1,N 
A(I,J)=A(I,J+N) 
CONTINUE 
DO  26  1=1, N 
WRITE(6,910) 
CONTINUE 
RETURN 
CONTINUE 
WRITE (6, 130) 
DO  120  1=1, N 
DO  120  J=1,N 
A(I,J)=1.0 
CONTINUE 
RETURN 

FORMAT ( 5 X, 'THE  MATRIX  IS  SINGULAR, NO  SOLUTION  HAS  BEEN  FOUND') 
FORMAT (5X, 'THE  MATRIX  IS  ILLCONDITIONED ' ) 
FORMAT(2X, 'ABEF. ' ,6E12.3) 

^12.4) 


(A(I,J),J=1,NPN) 


FORMAT(2X, 'AAFT. ' ,6E12 
END 
******SUBROUTIME  POUTOP************************************************** 

SUBROUTINE  POUTOP 
IMPLICIT  REAL*8(A"H,0-Z) 
RETURN 
END 
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